diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml
index 1adcb74ceb72d69e5108c5fb639a45c7d2a0e1ee..a47a0acd2c5f81ed46bf9f9f3ec642b750abfe97 100644
--- a/.gitlab-ci.yml
+++ b/.gitlab-ci.yml
@@ -1,76 +1,24 @@
-image: labrobotica/ceres:1.14
-
-before_script:
- - ls
- - apt-get update
- - apt-get install -y build-essential cmake
-
-# SPDLOG
-# - apt-get install -y libspdlog-dev
- - if [ -d spdlog ]; then
- - echo "directory exists"
- - if [ "$(ls -A ./spdlog)" ]; then
- - echo "directory not empty"
- - cd spdlog
- - git pull
- - else
- - echo "directory empty"
- - git clone https://github.com/gabime/spdlog.git
- - cd spdlog
- - fi
- - else
- - echo "directory inexistent"
- - git clone https://github.com/gabime/spdlog.git
- - cd spdlog
- - fi
- - git fetch
- - git checkout v0.17.0
+.build_and_test_template: &build_and_test_definition
- mkdir -pv build
- cd build
- - ls
- - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DSPDLOG_BUILD_TESTING=OFF ..
+ - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
- make -j$(nproc)
+ - ctest -j$(nproc)
+ # run demos
+ - ../bin/hello_wolf
+ - ../bin/hello_wolf_autoconf
- make install
- - cd ../..
-# YAML
-# - apt-get install -y libyaml-cpp-dev
- - if [ -d yaml-cpp ]; then
- - echo "directory exists"
- - if [ "$(ls -A ./yaml-cpp)" ]; then
- - echo "directory not empty"
- - cd yaml-cpp
- - git pull
- - else
- - echo "directory empty"
- - git clone https://github.com/jbeder/yaml-cpp.git
- - cd yaml-cpp
- - fi
- - else
- - echo "directory inexistent"
- - git clone https://github.com/jbeder/yaml-cpp.git
- - cd yaml-cpp
- - fi
- - mkdir -pv build
- - cd build
- - ls
- - cmake -DCMAKE_CXX_FLAGS="${CMAKE_CXX_FLAGS} -fPIC" -DYAML_CPP_BUILD_TESTS=OFF ..
- - make -j$(nproc)
- - make install
- - cd ../..
+build_and_test:xenial:
+ image: labrobotica/wolf_deps:16.04
+ except:
+ - master
+ script:
+ - *build_and_test_definition
-wolf_build_and_test:
- stage: build
+build_and_test:bionic:
+ image: labrobotica/wolf_deps:18.04
except:
- master
script:
- - mkdir -pv build
- - cd build
- - ls # we can check whether the directory was already full
- - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=ON -DBUILD_TESTS=ON ..
- - make -j$(nproc)
- - ctest -j$(nproc)
- # run demos
- - ../bin/hello_wolf
- - ../bin/hello_wolf_autoconf
- - make install
+ - *build_and_test_definition
\ No newline at end of file
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 4103c38189e7e0892959e7be9a52789920330c03..7777ac0a4ba15acbdb080c27ca0746fd57a43590 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -7,6 +7,7 @@ if(COMMAND cmake_policy)
cmake_policy(SET CMP0005 NEW)
cmake_policy(SET CMP0003 NEW)
endif(COMMAND cmake_policy)
+
# MAC OSX RPATH
SET(CMAKE_MACOSX_RPATH 1)
@@ -167,6 +168,7 @@ SET(HDRS_UTILS
include/core/utils/converter.h
include/core/utils/eigen_assert.h
include/core/utils/eigen_predicates.h
+ include/core/utils/graph_search.h
include/core/utils/loader.h
include/core/utils/logging.h
include/core/utils/make_unique.h
@@ -228,7 +230,8 @@ 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_kf_lmk_pose_3d_with_extrinsics.h
+ include/core/factor/factor_relative_pose_3d_with_extrinsics.h
+ include/core/factor/factor_velocity_local_direction_3d.h
)
SET(HDRS_FEATURE
include/core/feature/feature_base.h
@@ -246,9 +249,10 @@ 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_loopclosure.h
+ include/core/processor/processor_loop_closure.h
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2d.h
include/core/processor/processor_odom_3d.h
@@ -262,6 +266,7 @@ SET(HDRS_SENSOR
include/core/sensor/sensor_base.h
include/core/sensor/sensor_diff_drive.h
include/core/sensor/factory_sensor.h
+ include/core/sensor/sensor_model.h
include/core/sensor/sensor_odom_2d.h
include/core/sensor/sensor_odom_3d.h
include/core/sensor/sensor_pose.h
@@ -311,10 +316,11 @@ SET(SRCS_COMMON
SET(SRCS_MATH
)
SET(SRCS_UTILS
+ src/utils/check_log.cpp
src/utils/converter_utils.cpp
- src/utils/params_server.cpp
+ src/utils/graph_search.cpp
src/utils/loader.cpp
- src/utils/check_log.cpp
+ src/utils/params_server.cpp
)
SET(SRCS_CAPTURE
@@ -344,7 +350,8 @@ SET(SRCS_PROCESSOR
src/processor/motion_buffer.cpp
src/processor/processor_base.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_loopclosure.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
src/processor/processor_odom_3d.cpp
@@ -357,6 +364,7 @@ SET(SRCS_PROCESSOR
SET(SRCS_SENSOR
src/sensor/sensor_base.cpp
src/sensor/sensor_diff_drive.cpp
+ src/sensor/sensor_model.cpp
src/sensor/sensor_odom_2d.cpp
src/sensor/sensor_odom_3d.cpp
src/sensor/sensor_pose.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..115ea57f1803aa7a907c6e895e04d7731eab1560
--- /dev/null
+++ b/include/core/factor/factor_velocity_local_direction_3d.h
@@ -0,0 +1,149 @@
+
+#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);
+ _residuals[1] = 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/frame/frame_base.h b/include/core/frame/frame_base.h
index 848fa3f12f21e709286a61cd37edb4122fe361bc..7234284a5d00ebcb66f6f9a2efee2c38c9745bbd 100644
--- a/include/core/frame/frame_base.h
+++ b/include/core/frame/frame_base.h
@@ -102,6 +102,12 @@ class FrameBase : public NodeBase, public HasStateBlocks, public std::enable_sha
FrameBasePtr getNextFrame() const;
const CaptureBasePtrList& getCaptureList() const;
+ template <class C>
+ CaptureBasePtr getCaptureOfType() const;
+ CaptureBasePtr getCaptureOfType(const std::string& type) const;
+ template <class C>
+ CaptureBasePtrList getCapturesOfType() const;
+ CaptureBasePtrList getCapturesOfType(const std::string& type) const;
CaptureBasePtr getCaptureOf(const SensorBaseConstPtr _sensor_ptr) const;
CaptureBasePtr getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const;
CaptureBasePtrList getCapturesOf(const SensorBasePtr _sensor_ptr) const;
@@ -208,6 +214,25 @@ inline void FrameBase::setTrajectory(TrajectoryBasePtr _trj_ptr)
trajectory_ptr_ = _trj_ptr;
}
+template <class C>
+inline CaptureBasePtr FrameBase::getCaptureOfType() const
+{
+ for (CaptureBasePtr capture_ptr : getCaptureList())
+ if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
+ return capture_ptr;
+ return nullptr;
+}
+
+template <class C>
+inline CaptureBasePtrList FrameBase::getCapturesOfType() const
+{
+ CaptureBasePtrList captures;
+ for (CaptureBasePtr capture_ptr : getCaptureList())
+ if (std::dynamic_pointer_cast<C>(capture_ptr) != nullptr)
+ captures.push_back(capture_ptr);
+ return captures;
+}
+
} // namespace wolf
#endif
diff --git a/include/core/map/map_base.h b/include/core/map/map_base.h
index caac2f94eed78b033c153373f5d25f101fc2d833..93663bdf7fed2b4a8af6867c8e6226df6bd76571 100644
--- a/include/core/map/map_base.h
+++ b/include/core/map/map_base.h
@@ -28,7 +28,7 @@ class MapBase : public NodeBase, public std::enable_shared_from_this<MapBase>
MapBase();
~MapBase() override;
- private:
+ protected:
virtual LandmarkBasePtr addLandmark(LandmarkBasePtr _landmark_ptr);
virtual void removeLandmark(LandmarkBasePtr _landmark_ptr);
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/processor/processor_loop_closure.h b/include/core/processor/processor_loop_closure.h
new file mode 100644
index 0000000000000000000000000000000000000000..94af3fa1692397e533db0379a342a39edebb51e0
--- /dev/null
+++ b/include/core/processor/processor_loop_closure.h
@@ -0,0 +1,110 @@
+#ifndef _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
+#define _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H
+
+// Wolf related headers
+#include "core/processor/processor_base.h"
+
+namespace wolf{
+
+WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosure);
+
+struct ParamsProcessorLoopClosure : public ParamsProcessorBase
+{
+ int max_loops=-1;
+
+ ParamsProcessorLoopClosure() = default;
+ ParamsProcessorLoopClosure(std::string _unique_name, const ParamsServer& _server):
+ ParamsProcessorBase(_unique_name, _server)
+ {
+ max_loops = _server.getParam<int>(prefix + _unique_name + "/max_loops");
+ }
+
+ std::string print() const override
+ {
+ return "\n" + ParamsProcessorBase::print()
+ + "max_loops: " + std::to_string(max_loops) + "\n";
+ }
+};
+
+WOLF_STRUCT_PTR_TYPEDEFS(MatchLoopClosure);
+
+/** \brief Match between a capture and a capture
+ *
+ * Match between a capture and a capture (capture-capture correspondence)
+ *
+ */
+struct MatchLoopClosure
+{
+ CaptureBasePtr capture_reference; ///< Capture reference
+ CaptureBasePtr capture_target; ///< Capture target
+ double normalized_score; ///< normalized similarity score (0 is bad, 1 is good)
+};
+
+/** \brief General loop closure processor
+ *
+ * This is an abstract class.
+ * + You must define the following classes :
+ * - voteFindLoopClosures(CaptureBasePtr)
+ * - emplaceFeatures(CaptureBasePtr)
+ * - findLoopClosures(CaptureBasePtr)
+ * - validateLoop(CaptureBasePtr, CaptureBasePtr)
+ * - emplaceFactors(CaptureBasePtr, CaptureBasePtr)
+ * + You can override the following classes :
+ * - process(CaptureBasePtr)
+ */
+class ProcessorLoopClosure : public ProcessorBase
+{
+ protected:
+
+ ParamsProcessorLoopClosurePtr params_loop_closure_;
+
+ public:
+
+ ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
+
+ ~ProcessorLoopClosure() override = default;
+ void configure(SensorBasePtr _sensor) override { };
+
+ protected:
+
+ /** \brief Process a capture (linked to a frame)
+ * If voteFindLoopClosures() returns true, findLoopClosures() is called.
+ * emplaceFactors() is called for pairs of current capture and each capture returned by findLoopClosures()
+ */
+ virtual void process(CaptureBasePtr);
+
+ /** \brief Returns if findLoopClosures() has to be called for the given capture
+ */
+ virtual bool voteFindLoopClosures(CaptureBasePtr cap) = 0;
+
+ /** \brief detects and emplaces all features of the given capture
+ */
+ virtual void emplaceFeatures(CaptureBasePtr cap) = 0;
+
+ /** \brief Find captures that correspond to loop closures with the given capture
+ */
+ virtual std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) = 0;
+
+ /** \brief validates a loop closure
+ */
+ virtual bool validateLoopClosure(MatchLoopClosurePtr) = 0;
+
+ /** \brief emplaces the factor(s) corresponding to a Loop Closure between two captures
+ */
+ virtual void emplaceFactors(MatchLoopClosurePtr) = 0;
+
+ void processCapture(CaptureBasePtr) override;
+ void processKeyFrame(FrameBasePtr, const double&) override;
+
+ bool triggerInCapture(CaptureBasePtr _cap) const override { return true;};
+ bool triggerInKeyFrame(FrameBasePtr _frm, const double& _time_tol) const override { return true;};
+
+ bool storeKeyFrame(FrameBasePtr _frm) override { return false;};
+ bool storeCapture(CaptureBasePtr _cap) override { return false;};
+
+ bool voteForKeyFrame() const override { return false;};
+};
+
+} // namespace wolf
+
+#endif /* _WOLF_PROCESSOR_LOOP_CLOSURE_BASE_H */
diff --git a/include/core/processor/processor_loopclosure.h b/include/core/processor/processor_loopclosure.h
deleted file mode 100644
index 12e1fd8781627154315b4815a86105e1d404e86f..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_loopclosure.h
+++ /dev/null
@@ -1,183 +0,0 @@
-#ifndef _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
-#define _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H
-
-// Wolf related headers
-#include "core/processor/processor_base.h"
-
-namespace wolf{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorLoopClosure);
-
-struct ParamsProcessorLoopClosure : public ParamsProcessorBase
-{
- using ParamsProcessorBase::ParamsProcessorBase;
- // virtual ~ParamsProcessorLoopClosure() = default;
-
- // add neccesery parameters for loop closure initialisation here and initialize
- // them in constructor
-};
-
-/** \brief General loop closure processor
- *
- * This is an abstract class.
- * + You must define the following classes :
- * - voteComputeFeatures()
- * - voteSearchLoopClosure()
- * - computeFeatures()
- * - findLoopCandidate()
- * - createFactors()
- * + You can override the following classes :
- * - selectPairKC()
- * - validateLoop()
- * - processLoopClosure()
- *
- * It establishes factors XXX
- *
- * Should you need extra functionality for your derived types,
- * you can overload these two methods,
- *
- * - preProcess() { }
- * - postProcess() { }
- *
- * which are called at the beginning and at the end of process() respectively.
- */
-
-class ProcessorLoopClosure : public ProcessorBase
-{
-protected:
-
- ParamsProcessorLoopClosurePtr params_loop_closure_;
-
-public:
-
- ProcessorLoopClosure(const std::string& _type, int _dim, ParamsProcessorLoopClosurePtr _params_loop_closure);
-
- ~ProcessorLoopClosure() override = default;
- void configure(SensorBasePtr _sensor) override { };
-
-protected:
- /** \brief process an incoming capture
- *
- * The ProcessorLoopClosure is only triggered in KF (see triggerInCapture()) so this function is not called.
- */
- void processCapture(CaptureBasePtr) override {};
-
- /** \brief process an incoming key-frame
- *
- * Each derived processor should implement this function. It will be called if:
- * - A new KF arrived and triggerInKF() returned true.
- */
- void processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance) override;
-
- /** \brief trigger in capture
- *
- * The ProcessorLoopClosure only processes incoming KF, then it returns false.
- */
- bool triggerInCapture(CaptureBasePtr) const override {return false;}
-
- /** \brief trigger in key-frame
- *
- * Returns true if processKeyFrame() should be called after the provided KF arrived.
- */
- bool triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const override;
-
- /** \brief store key frame
- *
- * Returns true if the key frame should be stored
- */
- bool storeKeyFrame(FrameBasePtr) override;
-
- /** \brief store capture
- *
- * Returns true if the capture should be stored
- */
- bool storeCapture(CaptureBasePtr) override;
-
- /** \brief Called by process(). Tells if computeFeatures() will be called
- */
- virtual bool voteComputeFeatures() = 0;
-
- /** \brief Called by process(). Tells if findLoopCandidate() and createFactors() will be called
- *
- * WARNING : A LC can be searched only when voteComputeFeatures() return true
- */
- virtual bool voteSearchLoopClosure() = 0;
-
- /** \brief returns a KeyFrame-Capture pair compatible together (selected from the buffers)
- *
- * Should clear elements before the ones selected in buffers.
- * In the default implementation, we select the KF with the most recent TimeStamp
- * and that is compatible with at least a Capture
- */
- virtual std::pair<FrameBasePtr,CaptureBasePtr> selectPairKC(void);
-
- /** \brief add the Capture and all features needed to the corresponding KF
- *
- * If the loop closure process requires features associated to each capture,
- * the computations to create these features must be done here.
- *
- * Important: All detected features should be emplaced to the capture.
- *
- * Returns a bool that tells if features were successfully created
- */
- virtual bool detectFeatures(CaptureBasePtr cap) = 0;
-
- /** \brief Find a KF that would be a good candidate to close a loop
- * if validateLoop is not overwritten, a loop will be closed with the returned candidate
- * if no good candidate is found, return nullptr
- */
- virtual CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) = 0;
-
- /** \brief validate/discard a loop closure
- *
- * overwrite it if you want an additional test after findLoopCandidate()
- */
- virtual bool validateLoop(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) {return true;};
-
- /** \brief emplace the factor(s)
- *
- */
- virtual void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) = 0;
-
-
- /** Pre-process incoming Capture
- *
- * This is called by process() just after assigning incoming_ptr_ to a valid Capture.
- *
- * Overload this function to prepare stuff on derived classes.
- *
- * Typical uses of prePrecess() are:
- * - casting base types to derived types
- * - initializing counters, flags, or any derived variables
- * - initializing algorithms needed for processing the derived data
- */
- virtual void preProcess() { }
-
- /** Post-process
- *
- * This is called by process() after finishing the processing algorithm.
- *
- * Overload this function to post-process stuff on derived classes.
- *
- * Typical uses of postPrecess() are:
- * - resetting and/or clearing variables and/or algorithms at the end of processing
- * - drawing / printing / logging the results of the processing
- */
- virtual void postProcess() { }
-
- /** \brief Vote for KeyFrame generation
- *
- * If a KeyFrame criterion is validated, this function returns true,
- * meaning that it wants to create a KeyFrame at the \b last Capture.
- *
- * WARNING! This function only votes! It does not create KeyFrames!
- */
- bool voteForKeyFrame() const override
- {
- return false;
- };
-};
-
-} // namespace wolf
-
-#endif /* _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H */
diff --git a/include/core/sensor/sensor_base.h b/include/core/sensor/sensor_base.h
index f4cae62c31f2c8389107388bdebedbec72a68b5b..b80ac75c31d27014cb1affc8d9012e60b6476017 100644
--- a/include/core/sensor/sensor_base.h
+++ b/include/core/sensor/sensor_base.h
@@ -244,6 +244,8 @@ class SensorBase : public NodeBase, public HasStateBlocks, public std::enable_sh
bool state_blocks,
std::ostream& stream ,
std::string _tabs = "") const;
+ void printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const;
+
void print(int depth, //
bool constr_by, //
bool metric, //
diff --git a/include/core/sensor/sensor_model.h b/include/core/sensor/sensor_model.h
new file mode 100644
index 0000000000000000000000000000000000000000..5e2a37df70243f7d331dadde464fffd1d7f081ef
--- /dev/null
+++ b/include/core/sensor/sensor_model.h
@@ -0,0 +1,38 @@
+#ifndef SRC_SENSOR_MODEL_H_
+#define SRC_SENSOR_MODEL_H_
+
+//wolf includes
+#include "core/sensor/sensor_base.h"
+
+namespace wolf {
+
+WOLF_PTR_TYPEDEFS(SensorModel);
+
+class SensorModel : public SensorBase
+{
+ public:
+ SensorModel();
+ ~SensorModel() override;
+
+ static SensorBasePtr create(const std::string& _unique_name,
+ const ParamsServer& _server)
+ {
+ auto sensor = std::make_shared<SensorModel>();
+ sensor ->setName(_unique_name);
+ return sensor;
+ }
+
+ static SensorBasePtr create(const std::string& _unique_name,
+ const Eigen::VectorXd& _extrinsics,
+ const ParamsSensorBasePtr _intrinsics)
+ {
+ auto sensor = std::make_shared<SensorModel>();
+ sensor ->setName(_unique_name);
+ return sensor;
+ }
+};
+
+
+} /* namespace wolf */
+
+#endif /* SRC_SENSOR_POSE_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/include/core/state_block/state_composite.h b/include/core/state_block/state_composite.h
index 7989fd807eab08187c802e0386af02de56ef4f1b..d0ddf4e7fc453212f345640f746cca212170f3b7 100644
--- a/include/core/state_block/state_composite.h
+++ b/include/core/state_block/state_composite.h
@@ -31,7 +31,6 @@ class VectorComposite : public std::unordered_map < char, Eigen::VectorXd >
VectorComposite() {};
VectorComposite(const StateStructure& _s);
VectorComposite(const StateStructure& _s, const std::list<int>& _sizes);
- VectorComposite(const VectorComposite & v) : unordered_map<char, VectorXd>(v){};
/**
* \brief Construct from Eigen::VectorXd and structure
*
diff --git a/include/core/utils/graph_search.h b/include/core/utils/graph_search.h
new file mode 100644
index 0000000000000000000000000000000000000000..cf14ff844d981922848645774799f2b18655e06f
--- /dev/null
+++ b/include/core/utils/graph_search.h
@@ -0,0 +1,44 @@
+#ifndef GRAPH_SEARCH_H
+#define GRAPH_SEARCH_H
+
+#include "core/common/wolf.h"
+#include "core/frame/frame_base.h"
+#include "core/factor/factor_base.h"
+
+#include <map>
+
+namespace wolf
+{
+
+class GraphSearch
+{
+ private:
+
+ std::map<FrameBasePtr, std::pair<FactorBasePtr,FrameBasePtr>> parents_;
+
+ public:
+
+ GraphSearch();
+
+ ~GraphSearch();
+
+ FactorBasePtrList computeShortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist = 0);
+
+ std::set<FrameBasePtr> getNeighborFrames(const std::set<FrameBasePtr>& frms);
+
+ static FactorBasePtrList shortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist = 0)
+ {
+ GraphSearch graph_search;
+
+ return graph_search.computeShortestPath(frm1, frm2, max_graph_dist);
+ }
+
+};
+
+
+} // namespace wolf
+#endif
diff --git a/src/capture/capture_motion.cpp b/src/capture/capture_motion.cpp
index 16a535dc987deea66756f4c8468ac3005f2b3573..b87e4fb10c37d79ede036e72983faea5d7d12b7c 100644
--- a/src/capture/capture_motion.cpp
+++ b/src/capture/capture_motion.cpp
@@ -49,7 +49,7 @@ CaptureMotion::~CaptureMotion()
bool CaptureMotion::containsTimeStamp (const TimeStamp& _ts, double _time_tolerance)
{
- assert(_ts.ok());
+ assert(_ts.ok() and this->time_stamp_.ok());
// the same capture is within tolerance
if (this->time_stamp_ - _time_tolerance <= _ts && _ts <= this->time_stamp_ + _time_tolerance)
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/factor/factor_base.cpp b/src/factor/factor_base.cpp
index 5e3fef35efa980c310226d3976cf7d3387fbbd9d..df80bf00bc0f38d08d2b9c6806a7d0ef9a01a8c7 100644
--- a/src/factor/factor_base.cpp
+++ b/src/factor/factor_base.cpp
@@ -307,7 +307,7 @@ void FactorBase::setProblem(ProblemPtr _problem)
void FactorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
- _stream << _tabs << "Fac" << id() << " " << getType() << " -->";
+ _stream << _tabs << "Fac" << id() << " " << getType() << (getStatus() == FAC_ACTIVE ? "" : " INACTIVE") << " -->";
if ( getFrameOtherList() .empty()
&& getCaptureOtherList() .empty()
&& getFeatureOtherList() .empty()
diff --git a/src/frame/frame_base.cpp b/src/frame/frame_base.cpp
index 7df23b32b9d6f1bf876f796715ff6903fc6fd4bb..71e344552517ef659bb3ae60f66abbb95c25056f 100644
--- a/src/frame/frame_base.cpp
+++ b/src/frame/frame_base.cpp
@@ -165,6 +165,7 @@ FrameBasePtr FrameBase::getNextFrame() const
CaptureBasePtr FrameBase::addCapture(CaptureBasePtr _capt_ptr)
{
+ WOLF_WARN_COND(getCaptureOf(_capt_ptr->getSensor()) != nullptr, "FrameBase::addCapture adding new capture ", _capt_ptr->id(), " in a frame with another capture of the same sensor: ", getCaptureOf(_capt_ptr->getSensor())->id());
capture_list_.push_back(_capt_ptr);
return _capt_ptr;
}
@@ -174,6 +175,23 @@ void FrameBase::removeCapture(CaptureBasePtr _capt_ptr)
capture_list_.remove(_capt_ptr);
}
+CaptureBasePtr FrameBase::getCaptureOfType(const std::string& type) const
+{
+ for (CaptureBasePtr capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ return capture_ptr;
+ return nullptr;
+}
+
+CaptureBasePtrList FrameBase::getCapturesOfType(const std::string& type) const
+{
+ CaptureBasePtrList captures;
+ for (CaptureBasePtr capture_ptr : getCaptureList())
+ if (capture_ptr->getType() == type)
+ captures.push_back(capture_ptr);
+ return captures;
+}
+
CaptureBasePtr FrameBase::getCaptureOf(const SensorBasePtr _sensor_ptr, const std::string& type) const
{
for (CaptureBasePtr capture_ptr : getCaptureList())
@@ -205,6 +223,11 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr, cons
for (const FactorBasePtr& factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
return factor_ptr;
+
+ for (const FactorBasePtr& factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr && factor_ptr->getType() == type)
+ return factor_ptr;
+
return nullptr;
}
@@ -213,6 +236,11 @@ FactorBasePtr FrameBase::getFactorOf(const ProcessorBasePtr _processor_ptr) cons
for (const FactorBasePtr& factor_ptr : getConstrainedByList())
if (factor_ptr->getProcessor() == _processor_ptr)
return factor_ptr;
+
+ for (const FactorBasePtr& factor_ptr : getFactorList())
+ if (factor_ptr->getProcessor() == _processor_ptr)
+ return factor_ptr;
+
return nullptr;
}
diff --git a/src/processor/processor_base.cpp b/src/processor/processor_base.cpp
index 794699f4533a7d2f5e6271e8f31b05b0b36cbd69..28963ed644aacd240d2d01a0bb23e6280dc6619a 100644
--- a/src/processor/processor_base.cpp
+++ b/src/processor/processor_base.cpp
@@ -121,7 +121,7 @@ void ProcessorBase::link(SensorBasePtr _sen_ptr)
void ProcessorBase::setProblem(ProblemPtr _problem)
{
std::string str = "Processor works with " + std::to_string(dim_) + "D but problem is " + std::to_string(_problem->getDim()) + "D";
- assert((dim_ == 0 or dim_ == _problem->getDim()) && str.c_str());
+ assert(((dim_ == 0) or (dim_ == _problem->getDim())) && str.c_str());
if (_problem == nullptr or _problem == this->getProblem())
return;
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/src/processor/processor_loop_closure.cpp b/src/processor/processor_loop_closure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..486be875ac6eb306e6e0a7973742b7f3e52241c7
--- /dev/null
+++ b/src/processor/processor_loop_closure.cpp
@@ -0,0 +1,154 @@
+#include "core/processor/processor_loop_closure.h"
+
+namespace wolf
+{
+
+ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type,
+ int _dim,
+ ParamsProcessorLoopClosurePtr _params_loop_closure):
+ ProcessorBase(_type, _dim, _params_loop_closure),
+ params_loop_closure_(_params_loop_closure)
+{
+ //
+}
+
+void ProcessorLoopClosure::processCapture(CaptureBasePtr _capture)
+{
+ /* This function has 3 scenarios:
+ * 1. Capture already linked to a frame (in trajectory) -> process
+ * 2. Capture has a timestamp compatible with any stored frame -> link + process
+ * 3. Otherwise -> store capture (Note that more than one processor can be emplacing frames, so an older frame can arrive later than this one)
+ */
+
+ WOLF_DEBUG("ProcessorLoopClosure::processCapture capture ", _capture->id());
+
+ // CASE 1:
+ if (_capture->getFrame() and _capture->getFrame()->getTrajectory())
+ {
+ WOLF_DEBUG("CASE 1");
+
+ process(_capture);
+
+ // remove the frame and older frames
+ buffer_pack_kf_.removeUpTo(_capture->getFrame()->getTimeStamp());
+
+ return;
+ }
+
+ // Search for any stored frame within time tolerance of capture
+ auto frame_pack = buffer_pack_kf_.select(_capture->getTimeStamp(), params_->time_tolerance);
+
+ // CASE 2:
+ if (_capture->getFrame() == nullptr and frame_pack)
+ {
+ WOLF_DEBUG("CASE 2");
+
+ _capture->link(frame_pack->key_frame);
+
+ process(_capture);
+
+ // remove the frame and older frames
+ buffer_pack_kf_.removeUpTo(frame_pack->key_frame->getTimeStamp());
+
+ return;
+ }
+ // CASE 3:
+ WOLF_DEBUG("CASE 3");
+ buffer_capture_.add(_capture->getTimeStamp(), _capture);
+}
+
+void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _frame, const double& _time_tolerance)
+{
+ /* This function has 4 scenarios:
+ * 1. Frame already have a capture of the sensor -> process
+ * 2. Frame has a timestamp within time tolerances of any stored capture -> link + process
+ * 3. Frame is more recent than any stored capture -> store frame to be processed later in processCapture
+ * 4. Otherwise: The frame is not compatible with any stored capture -> discard frame
+ */
+
+ WOLF_DEBUG("ProcessorLoopClosure::processKeyFrame frame ", _frame->id());
+
+ // CASE 1:
+ auto cap = _frame->getCaptureOf(getSensor());
+ if (cap)
+ {
+ WOLF_DEBUG("CASE 1");
+
+ process(cap);
+
+ // remove the capture (if stored)
+ buffer_capture_.getContainer().erase(cap->getTimeStamp());
+
+ return;
+ }
+
+ // Search for any stored capture within time tolerance of frame
+ auto capture = buffer_capture_.select(_frame->getTimeStamp(), params_->time_tolerance);
+
+ // CASE 2:
+ if (capture and not capture->getFrame())
+ {
+ WOLF_DEBUG("CASE 2");
+
+ capture->link(_frame);
+
+ process(capture);
+
+ // remove the capture (if stored)
+ buffer_capture_.getContainer().erase(capture->getTimeStamp());
+
+ // remove old captures (10s of old captures are kept in case frames arrives unordered)
+ buffer_capture_.removeUpTo(_frame->getTimeStamp() - 10);
+
+ return;
+ }
+ // CASE 3:
+ if (buffer_capture_.selectLastAfter(_frame->getTimeStamp(), params_->time_tolerance) == nullptr)
+ {
+ WOLF_DEBUG("CASE 3");
+
+ // store frame
+ buffer_pack_kf_.add(_frame, _time_tolerance);
+
+ return;
+ }
+ // CASE 4:
+ WOLF_DEBUG("CASE 4");
+ // nothing (discard frame)
+}
+
+void ProcessorLoopClosure::process(CaptureBasePtr _capture)
+{
+ assert(_capture->getFrame() != nullptr && "ProcessorLoopClosure::process _capture not linked to _frame");
+ WOLF_DEBUG("ProcessorLoopClosure::process frame ", _capture->getFrame()->id(), " capture ", _capture->id());
+
+ // Detect and emplace features
+ WOLF_DEBUG("emplacing features...");
+ emplaceFeatures(_capture);
+
+ // Vote for loop closure search
+ if (voteFindLoopClosures(_capture))
+ {
+ WOLF_DEBUG("finding loop closures...");
+
+ // Find loop closures
+ auto match_lc_map = findLoopClosures(_capture);
+
+ WOLF_DEBUG(match_lc_map.size(), " loop closures found");
+
+ // Emplace factors for each LC if validated
+ auto n_loops = 0;
+ for (const auto& match_pair : match_lc_map)
+ if (validateLoopClosure(match_pair.second))
+ {
+ emplaceFactors(match_pair.second);
+ n_loops++;
+
+ if (params_loop_closure_->max_loops > 0 and
+ n_loops >= params_loop_closure_->max_loops)
+ break;
+ }
+ }
+}
+
+}// namespace wolf
diff --git a/src/processor/processor_loopclosure.cpp b/src/processor/processor_loopclosure.cpp
deleted file mode 100644
index 16e037b1c7634c4d0d7509fea51e075fbc8d0066..0000000000000000000000000000000000000000
--- a/src/processor/processor_loopclosure.cpp
+++ /dev/null
@@ -1,119 +0,0 @@
-/**
- * \file processor_loop_closure.h
- *
- * Created on: Mai 31, 2019
- * \author: Pierre Guetschel
- */
-
-#include "core/processor/processor_loopclosure.h"
-
-
-namespace wolf
-{
-
-ProcessorLoopClosure::ProcessorLoopClosure(const std::string& _type,
- int _dim,
- ParamsProcessorLoopClosurePtr _params_loop_closure):
- ProcessorBase(_type, _dim, _params_loop_closure),
- params_loop_closure_(_params_loop_closure)
-{
- //
-}
-
-//##############################################################################
-void ProcessorLoopClosure::processKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tolerance)
-{
- // the pre-process, if necessary, is implemented in the derived classes
- preProcess();
-
- if (voteComputeFeatures())
- {
- std::pair<FrameBasePtr,CaptureBasePtr> pairKC = selectPairKC();
-
- auto cap_1 = pairKC.second;
- auto kf_1 = pairKC.first;
-
- if (kf_1==nullptr || cap_1==nullptr) return;
- bool success_computeFeatures = detectFeatures(cap_1);
-
- // if succeded
- if (success_computeFeatures)
- {
- // link the capture to the KF (if not already linked)
- if (cap_1->getFrame() != kf_1)
- {
- assert(cap_1->getFrame() == nullptr && "capture already linked to a different frame"); //FIXME
- cap_1->link(kf_1);
- }
-
- // search loop closure
- if(voteSearchLoopClosure())
- {
- auto cap_2 = findLoopCandidate(cap_1);
- if (cap_2==nullptr)
- return;
- if (!validateLoop(cap_1, cap_2))
- return;
- if (cap_1->getFrame() == nullptr || cap_2->getFrame() == nullptr)
- {
- WOLF_WARN("ProcessorLoopClosureBase : tried to close a loop with captures linked to no KF");
- return;
- }
- if (cap_1->getFrame() == cap_2->getFrame())
- {
- WOLF_WARN("ProcessorLoopClosureBase : findLoopCandidate() returned two captures of the same frame");
- return;
- }
- emplaceFactors(cap_1, cap_2);
- }
- }
- }
-
- // the post-process, if necessary, is implemented in the derived classes
- postProcess();
-}
-
-bool ProcessorLoopClosure::triggerInKeyFrame(FrameBasePtr _keyframe_ptr, const double& _time_tol_other) const
-{
- return true;
-}
-bool ProcessorLoopClosure::storeKeyFrame(FrameBasePtr _frame_ptr)
-{
- return true;
-}
-bool ProcessorLoopClosure::storeCapture(CaptureBasePtr _cap_ptr)
-{
- return true;
-}
-
-/**
- * In the default implementation, we select the KF with the most recent TimeStamp
- * and that is compatible with at least a Capture
- */
-std::pair<FrameBasePtr,CaptureBasePtr> ProcessorLoopClosure::selectPairKC()
-{
- auto kf_container = buffer_pack_kf_.getContainer();
-
- if (kf_container.empty())
- return std::make_pair(nullptr, nullptr);
-
- for (auto kf_it=kf_container.begin(); kf_it!=kf_container.end(); ++kf_it)
- {
- CaptureBasePtr cap_ptr = buffer_capture_.select(kf_it->first, kf_it->second->time_tolerance);
- if (cap_ptr != nullptr)
- {
- // clear the buffers :
- buffer_capture_.removeUpTo(cap_ptr->getTimeStamp());
- buffer_pack_kf_.removeUpTo(kf_it->first);
- // return the KF-Cap pair :
- return std::make_pair(kf_it->second->key_frame, cap_ptr);
- }
- }
- return std::make_pair(nullptr, nullptr);
-}
-
-
-//##############################################################################
-
-
-}// namespace wolf
diff --git a/src/processor/processor_motion.cpp b/src/processor/processor_motion.cpp
index 11dee8587c0e1ba03a77f23902f6a7a18c00e6ec..9b7f8da44e1e58c86277a2cb689401426932c723 100644
--- a/src/processor/processor_motion.cpp
+++ b/src/processor/processor_motion.cpp
@@ -540,7 +540,7 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
// Get state of origin
const auto& x_origin = getOrigin()->getFrame()->getState(state_structure_);
- // Get most rescent motion
+ // Get most recent motion
const auto& motion = last_ptr_->getBuffer().back();
// Get delta preintegrated up to now
@@ -550,6 +550,8 @@ VectorComposite ProcessorMotion::getState(const StateStructure& _structure) cons
const auto& calib_preint = last_ptr_->getCalibrationPreint();
VectorComposite state;
+ //WOLF_INFO("processorMotion last timestamp: ", last_ptr_->getTimeStamp());
+ //WOLF_INFO("processorMotion origin timestamp: ", origin_ptr_->getTimeStamp());
if ( hasCalibration())
{
// Get current calibration -- from origin capture
@@ -894,6 +896,7 @@ CaptureMotionPtr ProcessorMotion::findCaptureContainingTimeStamp(const TimeStamp
capture = frame->getCaptureOf(sensor);
if (capture != nullptr)
{
+ assert(std::dynamic_pointer_cast<CaptureMotion>(capture) != nullptr);
// Rule 1 satisfied! We found a Capture belonging to this processor's Sensor ==> it is a CaptureMotion
capture_motion = std::static_pointer_cast<CaptureMotion>(capture);
diff --git a/src/sensor/sensor_base.cpp b/src/sensor/sensor_base.cpp
index 0d2d54638d145f333efa0e349990beda54d1b17e..0700d403f00f24fc4effcc8d176cdea9277be1a5 100644
--- a/src/sensor/sensor_base.cpp
+++ b/src/sensor/sensor_base.cpp
@@ -301,7 +301,7 @@ bool SensorBase::process(const CaptureBasePtr capture_ptr)
{
capture_ptr->setSensor(shared_from_this());
- for (const auto processor : processor_list_)
+ for (const auto& processor : processor_list_)
{
#ifdef PROFILING
@@ -429,6 +429,36 @@ void SensorBase::printHeader(int _depth, bool _constr_by, bool _metric, bool _st
printState(_metric, _state_blocks, _stream, _tabs);
}
+void SensorBase::printState (bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
+{
+ if (_metric && _state_blocks)
+ {
+ for (const auto &key : getStructure())
+ {
+ auto sb = getStateBlockDynamic(key);
+ if (sb)
+ _stream << _tabs << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] = ( "
+ << std::setprecision(3) << sb->getState().transpose() << " )" << " @ " << sb << std::endl;
+ }
+ }
+ else if (_metric)
+ {
+ _stream << _tabs << " " << (isFixed() ? "Fix" : "Est") << ",\t x = ( " << std::setprecision(3)
+ << getStateVector().transpose() << " )" << std::endl;
+ }
+ else if (_state_blocks)
+ {
+ _stream << _tabs << " " << "sb:";
+ for (const auto &key : getStructure())
+ {
+ const auto &sb = getStateBlockDynamic(key);
+ if (sb)
+ _stream << " " << key << "[" << (sb->isFixed() ? "Fix" : "Est") << "] @ " << sb;
+ }
+ _stream << std::endl;
+ }
+}
+
void SensorBase::print(int _depth, bool _constr_by, bool _metric, bool _state_blocks, std::ostream& _stream, std::string _tabs) const
{
printHeader(_depth, _constr_by, _metric, _state_blocks, _stream, _tabs);
@@ -509,4 +539,5 @@ bool SensorBase::check(CheckLog& _log, std::shared_ptr<NodeBase> _node_ptr, bool
return _log.is_consistent_;
}
+
} // namespace wolf
diff --git a/src/sensor/sensor_model.cpp b/src/sensor/sensor_model.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6eb00f021667436360821f27e5684a512eb425e4
--- /dev/null
+++ b/src/sensor/sensor_model.cpp
@@ -0,0 +1,24 @@
+#include "core/sensor/sensor_model.h"
+
+namespace wolf {
+
+
+SensorModel::SensorModel() :
+ SensorBase("SensorModel", nullptr, nullptr, nullptr, 6)
+{
+ //
+}
+
+SensorModel::~SensorModel()
+{
+ //
+}
+
+} // namespace wolf
+
+// Register in the FactorySensor
+#include "core/sensor/factory_sensor.h"
+namespace wolf {
+WOLF_REGISTER_SENSOR(SensorModel);
+WOLF_REGISTER_SENSOR_AUTO(SensorModel);
+}
diff --git a/src/state_block/state_composite.cpp b/src/state_block/state_composite.cpp
index 169bc82ca8edd1fa751bac1e309aa2ce1eb95379..04658a0fb6ccc9fd20412e650709d0d9320e169c 100644
--- a/src/state_block/state_composite.cpp
+++ b/src/state_block/state_composite.cpp
@@ -112,9 +112,9 @@ wolf::VectorComposite operator +(const wolf::VectorComposite &_x, const wolf::Ve
return xpy;
}
-wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::VectorComposite &_y)
+VectorComposite operator -(const VectorComposite &_x, const VectorComposite &_y)
{
- wolf::VectorComposite xpy;
+ VectorComposite xpy;
for (const auto& pair_i_xi : _x)
{
const auto& i = pair_i_xi.first;
@@ -124,6 +124,17 @@ wolf::VectorComposite operator -(const wolf::VectorComposite &_x, const wolf::Ve
return xpy;
}
+VectorComposite operator -(const wolf::VectorComposite &_x)
+{
+ wolf::VectorComposite m;
+ for (const auto& pair_i_xi : _x)
+ {
+ const auto& i = pair_i_xi.first;
+ m.emplace(i, - _x.at(i));
+ }
+ return m;
+}
+
void VectorComposite::set (const VectorXd& _v, const StateStructure& _structure, const std::list<int>& _sizes)
{
int index = 0;
@@ -145,17 +156,6 @@ void VectorComposite::setZero()
pair_key_vec.second.setZero();
}
-wolf::VectorComposite operator -(const wolf::VectorComposite &_x)
-{
- wolf::VectorComposite m;
- for (const auto& pair_i_xi : _x)
- {
- const auto& i = pair_i_xi.first;
- m.emplace(i, - _x.at(i));
- }
- return m;
-}
-
////// MATRIX COMPOSITE //////////
bool MatrixComposite::emplace(const char &_row, const char &_col, const Eigen::MatrixXd &_mat_blk)
diff --git a/src/utils/graph_search.cpp b/src/utils/graph_search.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..d2876701b84d3acabf5e4e9c761a49570bffc2fe
--- /dev/null
+++ b/src/utils/graph_search.cpp
@@ -0,0 +1,119 @@
+#include "core/utils/graph_search.h"
+
+using namespace wolf;
+
+GraphSearch::GraphSearch() :
+ parents_()
+{
+
+}
+
+GraphSearch::~GraphSearch()
+{
+
+}
+
+FactorBasePtrList GraphSearch::computeShortestPath(FrameBasePtr frm1,
+ FrameBasePtr frm2,
+ const unsigned int max_graph_dist)
+{
+ //WOLF_INFO("GraphSearch::computeShortestPath: from frame ", frm1->id(), " to frame ", frm2->id());
+
+ std::set<FrameBasePtr> frm_neigs({frm1});
+ parents_[frm1] = std::pair<FactorBasePtr,FrameBasePtr>(nullptr, nullptr);
+ unsigned int depth = 0;
+
+ //WOLF_INFO(frm1->id());
+
+ while (not frm_neigs.empty())
+ {
+ frm_neigs = getNeighborFrames(frm_neigs);
+ depth++;
+
+ //if (not frm_neigs.empty())
+ //{
+ // std::string frm_neigs_str(depth, '.');
+ // for (auto frm : frm_neigs)
+ // frm_neigs_str += std::to_string(frm->id()) + std::string(" ");
+ // WOLF_INFO(frm_neigs_str);
+ //}
+
+ // finish
+ if (frm_neigs.count(frm2) != 0)
+ {
+ //WOLF_INFO("Frame ", frm2->id(), " found!");
+
+ assert(parents_.count(frm2) != 0);
+ FactorBasePtrList factor_path;
+ auto frm_it = frm2;
+
+ while (frm_it != frm1)
+ {
+ factor_path.push_back(parents_.at(frm_it).first);
+ frm_it = parents_.at(frm_it).second;
+ }
+
+ return factor_path;
+ }
+
+ // stop
+ if (max_graph_dist > 0 and depth == max_graph_dist)
+ break;
+ }
+ //WOLF_INFO("Path to frame ", frm2->id(), " NOT found!");
+
+ return FactorBasePtrList();
+}
+
+std::set<FrameBasePtr> GraphSearch::getNeighborFrames(const std::set<FrameBasePtr>& frms)
+{
+ std::set<FrameBasePtr> frm_neigs;
+
+ for (auto frm : frms)
+ {
+ // get constrained by factors
+ FactorBasePtrList facs_by = frm->getConstrainedByList();
+
+ // Iterate over all factors_by
+ for (auto && fac_by : facs_by)
+ {
+ //WOLF_INFO_COND(fac_by, "fac_by: ", fac_by->id());
+ //WOLF_INFO_COND(fac_by->getFrame(), "fac_by->getFrame(): ", fac_by->getFrame()->id());
+ if (fac_by and
+ fac_by->getFrame() and
+ parents_.count(fac_by->getFrame()) == 0)
+ {
+ //WOLF_INFO("registering");
+ frm_neigs.insert(fac_by->getFrame());
+ parents_[fac_by->getFrame()] = std::pair<FactorBasePtr,FrameBasePtr>(fac_by, frm);
+ }
+ }
+
+ // get the factors of this frame
+ FactorBasePtrList facs_own;
+ frm->getFactorList(facs_own);
+
+ // Iterate over all factors_own
+ for (auto && fac_own : facs_own)
+ {
+ //WOLF_INFO_COND(fac_own, "fac_own: ", fac_own->id());
+ //WOLF_INFO_COND(fac_own->getFrameOtherList().empty(), "fac_own->getFrameOtherList() is empty");
+ if (fac_own and not fac_own->getFrameOtherList().empty())
+ for (auto frm_other_w : fac_own->getFrameOtherList())
+ {
+ auto frm_other = frm_other_w.lock();
+ //WOLF_INFO_COND(frm_other, "frm_other ", frm_other->id());
+ if (frm_other and
+ parents_.count(frm_other) == 0)
+ {
+ //WOLF_INFO("registering");
+ frm_neigs.insert(frm_other);
+ parents_[frm_other] = std::pair<FactorBasePtr,FrameBasePtr>(fac_own, frm);
+ }
+ }
+ }
+ }
+
+ return frm_neigs;
+}
+
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 4988d2a44860219f3cbf87d7331a01deaa057a0d..033a2c1222d3ad85f27178383e9bd7d765e58f54 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -80,6 +80,10 @@ target_link_libraries(gtest_feature_base ${PLUGIN_NAME})
wolf_add_gtest(gtest_frame_base gtest_frame_base.cpp)
target_link_libraries(gtest_frame_base ${PLUGIN_NAME})
+# GraphSearch class test
+wolf_add_gtest(gtest_graph_search gtest_graph_search.cpp)
+target_link_libraries(gtest_graph_search ${PLUGIN_NAME})
+
# HasStateBlocks classes test
wolf_add_gtest(gtest_has_state_blocks gtest_has_state_blocks.cpp)
target_link_libraries(gtest_has_state_blocks ${PLUGIN_NAME})
@@ -217,6 +221,10 @@ target_link_libraries(gtest_factor_relative_pose_3d ${PLUGIN_NAME})
wolf_add_gtest(gtest_factor_relative_pose_3d_with_extrinsics gtest_factor_relative_pose_3d_with_extrinsics.cpp)
target_link_libraries(gtest_factor_relative_pose_3d_with_extrinsics ${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,12 +238,16 @@ 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})
-# ProcessorLoopClosureBase class test
-wolf_add_gtest(gtest_processor_loopclosure gtest_processor_loopclosure.cpp)
-target_link_libraries(gtest_processor_loopclosure ${PLUGIN_NAME})
+# ProcessorLoopClosure class test
+wolf_add_gtest(gtest_processor_loop_closure gtest_processor_loop_closure.cpp)
+target_link_libraries(gtest_processor_loop_closure ${PLUGIN_NAME})
# ProcessorFrameNearestNeighborFilter class test
# wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2d gtest_processor_frame_nearest_neighbor_filter_2d.cpp)
diff --git a/test/dummy/processor_loop_closure_dummy.h b/test/dummy/processor_loop_closure_dummy.h
new file mode 100644
index 0000000000000000000000000000000000000000..388f2f0dd5f13624d21115a9351680ceb92ce743
--- /dev/null
+++ b/test/dummy/processor_loop_closure_dummy.h
@@ -0,0 +1,93 @@
+#ifndef TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_
+#define TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_
+
+#include "core/processor/processor_loop_closure.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
+
+// dummy class:
+class ProcessorLoopClosureDummy : public ProcessorLoopClosure
+{
+ public:
+ ProcessorLoopClosureDummy(ParamsProcessorLoopClosurePtr _params) :
+ ProcessorLoopClosure("ProcessorLoopClosureDummy", 2, _params)
+ {
+ }
+
+ protected:
+ bool voteFindLoopClosures(CaptureBasePtr cap) override { return true;};
+ bool validateLoopClosure(MatchLoopClosurePtr match) override { return true;};
+
+ void emplaceFeatures(CaptureBasePtr cap) override
+ {
+ // feature = frame pose
+ FeatureBase::emplace<FeatureBase>(cap,
+ "FeatureLoopClosureDummy",
+ cap->getFrame()->getState().vector("PO"),
+ MatrixXd::Identity(3,3));
+ }
+
+ std::map<double,MatchLoopClosurePtr> findLoopClosures(CaptureBasePtr _capture) override
+ {
+ std::map<double,MatchLoopClosurePtr> match_lc_map;
+
+ auto old_frame = _capture->getFrame()->getPreviousFrame();
+ while (old_frame)
+ {
+ // match if features (frames psoe) are close enough
+ for (auto cap : old_frame->getCaptureList())
+ for (auto feat : cap->getFeatureList())
+ if (feat->getType() == "FeatureLoopClosureDummy" and
+ (feat->getMeasurement() - _capture->getFeatureList().front()->getMeasurement()).norm() < 1e-3)
+ {
+ MatchLoopClosurePtr match = std::make_shared<MatchLoopClosure>();
+ match->capture_reference = cap;
+ match->capture_target = _capture;
+ match->normalized_score = 1;
+
+ while (match_lc_map.count(match->normalized_score))
+ match->normalized_score -= 1e-9;
+
+ match_lc_map.emplace(match->normalized_score, match);
+ }
+
+ old_frame = old_frame->getPreviousFrame();
+ }
+
+ return match_lc_map;
+ }
+
+ void emplaceFactors(MatchLoopClosurePtr match) override
+ {
+ FeatureBasePtr feat_2;
+ for (auto feat : match->capture_target->getFeatureList())
+ if (feat->getType() == "FeatureLoopClosureDummy")
+ {
+ feat_2 = feat;
+ break;
+ }
+
+ FactorBase::emplace<FactorRelativePose2d>(feat_2, feat_2,
+ match->capture_reference->getFrame(),
+ shared_from_this(),
+ false,
+ TOP_LOOP);
+ }
+
+ public:
+ unsigned int getNStoredFrames()
+ {
+ return buffer_pack_kf_.getContainer().size();
+ }
+
+ unsigned int getNStoredCaptures()
+ {
+ return buffer_capture_.getContainer().size();
+ }
+};
+
+
+#endif /* TEST_DUMMY_PROCESSOR_LOOP_CLOSURE_DUMMY_H_ */
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_graph_search.cpp b/test/gtest_graph_search.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..9693846e7994bf071a1aed368c99626c2311d43a
--- /dev/null
+++ b/test/gtest_graph_search.cpp
@@ -0,0 +1,225 @@
+/*
+ * gtest_graph_search.cpp
+ *
+ * Created on: Jul, 2021
+ * Author: jvallve
+ */
+
+#include "core/utils/utils_gtest.h"
+
+#include "core/problem/problem.h"
+#include "core/capture/capture_void.h"
+#include "core/factor/factor_relative_pose_2d.h"
+#include "core/utils/graph_search.h"
+
+#include <iostream>
+#include <thread>
+
+using namespace wolf;
+using namespace Eigen;
+
+class GraphSearchTest : public testing::Test
+{
+ public:
+ ProblemPtr problem;
+
+ void SetUp() override
+ {
+ problem = Problem::create("PO", 2);
+ }
+
+ FrameBasePtr emplaceFrame(const TimeStamp& ts)
+ {
+ return problem->emplaceFrame(ts, Vector3d::Zero());
+ }
+
+ FactorBasePtr createFactor(FrameBasePtr frm1, FrameBasePtr frm2)
+ {
+ auto C12 = CaptureBase::emplace<CaptureVoid>(frm2, frm2->getTimeStamp(), nullptr);
+ auto f12 = FeatureBase::emplace<FeatureBase>(C12, "odom", Vector3d::Zero(), Matrix3d::Identity());
+ return FactorBase::emplace<FactorRelativePose2d>(f12, f12, frm1, nullptr, false, TOP_MOTION);
+ }
+};
+
+TEST_F(GraphSearchTest, setup)
+{
+ ASSERT_TRUE(problem->check());
+}
+
+TEST_F(GraphSearchTest, nonsense11)
+{
+ auto F1 = emplaceFrame(1);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F1, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, single12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_EQ(fac_list.front(), fac12);
+}
+
+TEST_F(GraphSearchTest, single21)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F2, F1, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_EQ(fac_list.front(), fac12);
+}
+
+TEST_F(GraphSearchTest, double12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto fac12 = createFactor(F1,F2);
+ auto fac12b = createFactor(F1,F2);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_EQ(fac_list.size(), 1);
+ ASSERT_TRUE(fac_list.front() == fac12 or fac_list.front() == fac12b);
+}
+
+TEST_F(GraphSearchTest, no_solution12)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto fac23 = createFactor(F2,F3);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F2, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, no_solution21)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto fac23 = createFactor(F2,F3);
+
+ auto fac_list = GraphSearch::shortestPath(F2, F1, 10);
+
+ ASSERT_TRUE(fac_list.empty());
+}
+
+TEST_F(GraphSearchTest, large)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac45 = createFactor(F4,F5);
+ auto fac56 = createFactor(F5,F6);
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 8);
+
+ ASSERT_EQ(fac_list.size(), 8);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F1, F9, 7);
+
+ ASSERT_EQ(fac_list_2.size(), 0);
+}
+
+TEST_F(GraphSearchTest, large_no_solution)
+{
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac45 = createFactor(F4,F5);
+
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 10);
+
+ ASSERT_EQ(fac_list.size(), 0);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F9, F1, 10);
+
+ ASSERT_EQ(fac_list_2.size(), 0);
+}
+
+TEST_F(GraphSearchTest, large_dense)
+{
+ /* ------- ---------------
+ * | | | |
+ * 1---2---3---4---5---6---7---8---9
+ * | |
+ * -----------
+ */
+
+ auto F1 = emplaceFrame(1);
+ auto F2 = emplaceFrame(2);
+ auto F3 = emplaceFrame(3);
+ auto F4 = emplaceFrame(4);
+ auto F5 = emplaceFrame(5);
+ auto F6 = emplaceFrame(6);
+ auto F7 = emplaceFrame(7);
+ auto F8 = emplaceFrame(8);
+ auto F9 = emplaceFrame(9);
+
+ auto fac12 = createFactor(F1,F2);
+ auto fac13 = createFactor(F1,F3);
+ auto fac23 = createFactor(F2,F3);
+ auto fac34 = createFactor(F3,F4);
+ auto fac36 = createFactor(F3,F6);
+ auto fac45 = createFactor(F4,F5);
+ auto fac56 = createFactor(F5,F6);
+ auto fac59 = createFactor(F5,F9);
+ auto fac67 = createFactor(F6,F7);
+ auto fac78 = createFactor(F7,F8);
+ auto fac89 = createFactor(F8,F9);
+
+ auto fac_list = GraphSearch::shortestPath(F1, F9, 10);
+
+ ASSERT_EQ(fac_list.size(), 4);
+
+ auto fac_list_2 = GraphSearch::shortestPath(F9, F1, 10);
+
+ ASSERT_EQ(fac_list_2.size(), 4);
+}
+
+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();
+}
diff --git a/test/gtest_processor_loop_closure.cpp b/test/gtest_processor_loop_closure.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..aec8092de22eaee6a4577b43d8b7a10f8528582b
--- /dev/null
+++ b/test/gtest_processor_loop_closure.cpp
@@ -0,0 +1,327 @@
+
+#include "core/utils/utils_gtest.h"
+#include "core/problem/problem.h"
+#include "core/capture/capture_base.h"
+#include "core/factor/factor_relative_pose_2d.h"
+
+#include "dummy/processor_loop_closure_dummy.h"
+
+// STL
+#include <iterator>
+#include <iostream>
+
+using namespace wolf;
+using namespace Eigen;
+
+class ProcessorLoopClosureTest : public testing::Test
+{
+ protected:
+ // Wolf problem
+ ProblemPtr problem = Problem::create("PO", 2);
+ SensorBasePtr sensor;
+ ProcessorLoopClosureDummyPtr processor;
+
+ virtual void SetUp()
+ {
+ // Emplace sensor
+ sensor = SensorBase::emplace<SensorBase>(problem->getHardware(),
+ "SensorBase",
+ std::make_shared<StateBlock>(Vector2d::Zero()),
+ std::make_shared<StateBlock>(Vector1d::Zero()),
+ nullptr,
+ 2);
+
+ // Emplace processor
+ ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
+ params->time_tolerance = 0.5;
+ processor = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sensor,
+ params);
+ }
+
+ FrameBasePtr emplaceFrame(TimeStamp ts, const Vector3d& x)
+ {
+ // new frame
+ return problem->emplaceFrame(ts, x);
+ }
+
+ CaptureBasePtr emplaceCapture(FrameBasePtr frame)
+ {
+ // new capture
+ return CaptureBase::emplace<CaptureBase>(frame,
+ "CaptureBase",
+ frame->getTimeStamp(),
+ sensor);
+ }
+
+ CaptureBasePtr createCapture(TimeStamp ts)
+ {
+ // new capture
+ return std::make_shared<CaptureBase>("CaptureBase",
+ ts,
+ sensor);
+ }
+};
+
+TEST_F(ProcessorLoopClosureTest, installProcessor)
+{
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, frame_stored)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, capture_stored)
+{
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase1)
+{
+ // emplace frame and capture
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto cap1 = emplaceCapture(frm1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase2)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_EQ(cap1->getFrame(), frm1); // capture processed by the processor
+ EXPECT_EQ(cap1->getFeatureList().size(), 1); // capture processed by the processor
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase3)
+{
+ // new frame
+ auto frm1 = emplaceFrame(2, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(1);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 1);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackCase4)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ // new capture
+ auto cap1 = createCapture(2);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 1);
+}
+
+TEST_F(ProcessorLoopClosureTest, captureCallbackMatch)
+{
+ // new frame
+ auto frm1 = emplaceFrame(1, Vector3d::Zero());
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ auto frm3 = emplaceFrame(3, Vector3d::Zero());
+ auto frm4 = emplaceFrame(4, Vector3d::Zero());
+ auto frm5 = emplaceFrame(5, Vector3d::Zero());
+ // new captures
+ auto cap4 = createCapture(4);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm1, nullptr, 0.5);
+ problem->keyFrameCallback(frm2, nullptr, 0.5);
+ problem->keyFrameCallback(frm3, nullptr, 0.5);
+ problem->keyFrameCallback(frm4, nullptr, 0.5);
+ problem->keyFrameCallback(frm5, nullptr, 0.5);
+
+ // captureCallback
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(frm1->getCaptureList().size(), 0);
+ EXPECT_EQ(frm2->getCaptureList().size(), 0);
+ EXPECT_EQ(frm3->getCaptureList().size(), 0);
+ EXPECT_EQ(frm4->getCaptureList().size(), 1);
+ EXPECT_EQ(frm5->getCaptureList().size(), 0);
+
+ EXPECT_TRUE(cap4->getFrame() == frm4);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 1); // all oldest frames are removed from buffer
+ EXPECT_EQ(processor->getNStoredCaptures(), 0);
+}
+
+TEST_F(ProcessorLoopClosureTest, keyFrameCallbackMatch)
+{
+ // new frame
+ auto frm2 = emplaceFrame(2, Vector3d::Zero());
+ // new captures
+ auto cap1 = createCapture(1);
+ auto cap2 = createCapture(2);
+ auto cap3 = createCapture(3);
+ auto cap4 = createCapture(4);
+ auto cap5 = createCapture(5);
+
+ // captureCallback
+ processor->captureCallback(cap1);
+ processor->captureCallback(cap2);
+ processor->captureCallback(cap3);
+ processor->captureCallback(cap4);
+ processor->captureCallback(cap5);
+
+ // keyframecallback
+ problem->keyFrameCallback(frm2, nullptr, 0.5);
+
+ EXPECT_TRUE(cap1->getFrame() == nullptr);
+ EXPECT_TRUE(cap2->getFrame() == frm2);
+ EXPECT_TRUE(cap3->getFrame() == nullptr);
+ EXPECT_TRUE(cap4->getFrame() == nullptr);
+ EXPECT_TRUE(cap5->getFrame() == nullptr);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().size(), 0);
+ EXPECT_EQ(cap5->getFeatureList().size(), 0);
+
+ EXPECT_EQ(processor->getNStoredFrames(), 0);
+ EXPECT_EQ(processor->getNStoredCaptures(), 4);
+}
+
+TEST_F(ProcessorLoopClosureTest, emplaceFactors)
+{
+ // emplace frame and capture
+ auto cap1 = emplaceCapture(emplaceFrame(1, Vector3d::Zero()));
+ processor->captureCallback(cap1);
+
+ auto cap2 = emplaceCapture(emplaceFrame(2, Vector3d::Ones()));
+ processor->captureCallback(cap2);
+
+ auto cap3 = emplaceCapture(emplaceFrame(3, 2*Vector3d::Ones()));
+ processor->captureCallback(cap3);
+
+ auto cap4 = emplaceCapture(emplaceFrame(4, Vector3d::Zero()));
+ processor->captureCallback(cap4);
+
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().size(), 1);
+ EXPECT_EQ(cap2->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap3->getFrame()->getConstrainedByList().size(), 0);
+ EXPECT_EQ(cap4->getFrame()->getConstrainedByList().size(), 0);
+
+ EXPECT_EQ(cap1->getFeatureList().size(), 1);
+ EXPECT_EQ(cap2->getFeatureList().size(), 1);
+ EXPECT_EQ(cap3->getFeatureList().size(), 1);
+ EXPECT_EQ(cap4->getFeatureList().size(), 1);
+
+ EXPECT_EQ(cap1->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap2->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap3->getFeatureList().front()->getFactorList().size(), 0);
+ EXPECT_EQ(cap4->getFeatureList().front()->getFactorList().size(), 1);
+
+ EXPECT_EQ(cap1->getFrame()->getConstrainedByList().front(), cap4->getFeatureList().front()->getFactorList().front());
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}
diff --git a/test/gtest_processor_loopclosure.cpp b/test/gtest_processor_loopclosure.cpp
deleted file mode 100644
index 4e50c870348da1877b3a599c48158730e2df47ab..0000000000000000000000000000000000000000
--- a/test/gtest_processor_loopclosure.cpp
+++ /dev/null
@@ -1,104 +0,0 @@
-
-#include "core/utils/utils_gtest.h"
-#include "core/problem/problem.h"
-#include "core/capture/capture_void.h"
-
-#include "core/processor/processor_loopclosure.h"
-
-
-// STL
-#include <iterator>
-#include <iostream>
-
-using namespace wolf;
-using namespace Eigen;
-
-
-WOLF_PTR_TYPEDEFS(ProcessorLoopClosureDummy);
-
-// dummy class:
-class ProcessorLoopClosureDummy : public ProcessorLoopClosure
-{
-private:
- bool* factor_created;
-
-public:
- ProcessorLoopClosureDummy(ParamsProcessorLoopClosurePtr _params_loop_closure, bool& factor_created):
- ProcessorLoopClosure("LOOP CLOSURE DUMMY", 0, _params_loop_closure),
- factor_created(&factor_created){};
- std::pair<FrameBasePtr,CaptureBasePtr> public_selectPairKC(){ return selectPairKC();};
-
-protected:
- bool voteComputeFeatures() override { return true;};
- bool voteSearchLoopClosure() override { return true;};
- bool detectFeatures(CaptureBasePtr cap) override { return true;};
- CaptureBasePtr findLoopCandidate(CaptureBasePtr _capture) override
- {
- for (FrameBasePtr kf : *getProblem()->getTrajectory())
- for (CaptureBasePtr cap : kf->getCaptureList())
- if (cap != _capture)
- return cap;
- return nullptr;
- };
- void emplaceFactors(CaptureBasePtr _capture_1, CaptureBasePtr _capture_2) override
- {
- std::cout << "factor created\n";
- *factor_created = true;
- };
-};
-
-
-
-TEST(ProcessorLoopClosure, installProcessor)
-{
- using namespace wolf;
- using std::shared_ptr;
- using std::make_shared;
- using std::static_pointer_cast;
- using Eigen::Vector2d;
-
- bool factor_created = false;
-
-
- double dt = 0.01;
-
- // Wolf problem
- ProblemPtr problem = Problem::create("PO", 2);
-
- // Install tracker (sensor and processor)
- auto sens_lc = SensorBase::emplace<SensorBase>(problem->getHardware(),
- "SENSOR BASE",
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(1)),
- std::make_shared<StateBlock>(Eigen::VectorXd::Zero(2)), 2);
- ParamsProcessorLoopClosurePtr params = std::make_shared<ParamsProcessorLoopClosure>();
- auto proc_lc = ProcessorBase::emplace<ProcessorLoopClosureDummy>(sens_lc, params, factor_created);
- std::cout << "sensor & processor created and added to wolf problem" << std::endl;
-
- // initialize
- TimeStamp t(0.0);
- // Vector3d x(0,0,0);
- VectorComposite x(Vector3d(0,0,0), "PO", {2,1});
- // Matrix3d P = Matrix3d::Identity() * 0.1;
- VectorComposite P(Vector3d(sqrt(0.1),sqrt(0.1),sqrt(0.1)), "PO", {2,1});
- problem->setPriorFactor(x, P, t, dt/2); // KF1
-
-
- // new KF
- t += dt;
- auto kf = problem->emplaceFrame(t, x); //KF2
- // emplace a capture in KF
- auto capt_lc = CaptureBase::emplace<CaptureVoid>(kf, t, sens_lc);
- proc_lc->captureCallback(capt_lc);
-
- // callback KF
- proc_lc->keyFrameCallback(kf, dt/2);
-
- ASSERT_TRUE(factor_created);
-}
-
-int main(int argc, char **argv)
-{
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}