diff --git a/CMakeLists.txt b/CMakeLists.txt
index c3c6182415cba0d9eec234f294bed49381be7c3e..4b0d9a62d98febe8f8b4b3761fa5b857c67c4cb1 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -249,9 +249,7 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_capture_holder.h
include/core/processor/processor_diff_drive.h
include/core/processor/processor_factory.h
- include/core/processor/processor_frame_nearest_neighbor_filter.h
include/core/processor/processor_logging.h
- include/core/processor/processor_loopclosure_base.h
include/core/processor/processor_loopclosure_base2.h
include/core/processor/processor_motion.h
include/core/processor/processor_odom_2D.h
@@ -346,8 +344,6 @@ SET(SRCS_PROCESSOR
src/processor/processor_base.cpp
src/processor/processor_capture_holder.cpp
src/processor/processor_diff_drive.cpp
- src/processor/processor_frame_nearest_neighbor_filter.cpp
- src/processor/processor_loopclosure_base.cpp
src/processor/processor_loopclosure_base2.cpp
src/processor/processor_motion.cpp
src/processor/processor_odom_2D.cpp
diff --git a/include/core/processor/processor_frame_nearest_neighbor_filter.h b/include/core/processor/processor_frame_nearest_neighbor_filter.h
deleted file mode 100644
index dffee75750a452891432aa255447ad1ffb8491b2..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_frame_nearest_neighbor_filter.h
+++ /dev/null
@@ -1,128 +0,0 @@
-#ifndef _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
-#define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H
-
-// Wolf related headers
-#include "core/processor/processor_loopclosure_base.h"
-#include "core/state_block/state_block.h"
-#include "core/utils/params_server.hpp"
-
-namespace wolf{
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsFrameNearestNeighborFilter)
-WOLF_PTR_TYPEDEFS(ProcessorFrameNearestNeighborFilter)
-
-enum class LoopclosureDistanceType : std::size_t
-{
- LC_POINT_ELLIPSE = 1, // 2D
- LC_ELLIPSE_ELLIPSE, // 2D
- LC_POINT_ELLIPSOID, // 3D
- LC_ELLIPSOID_ELLIPSOID, // 3D
- LC_MAHALANOBIS_DISTANCE // 2D & 3D
-};
-
-struct ProcessorParamsFrameNearestNeighborFilter : public ProcessorParamsLoopClosure
-{
- using DistanceType = LoopclosureDistanceType;
-
- ProcessorParamsFrameNearestNeighborFilter() :
- buffer_size_(10),
- sample_step_degree_(10),
- distance_type_(LoopclosureDistanceType::LC_POINT_ELLIPSE),
- probability_(5.991) { }
-
- ProcessorParamsFrameNearestNeighborFilter(
- const ProcessorParamsFrameNearestNeighborFilter& o) :
- buffer_size_(o.buffer_size_),
- sample_step_degree_(o.sample_step_degree_),
- distance_type_(o.distance_type_),
- probability_(o.probability_)
- {
- //
- }
- ProcessorParamsFrameNearestNeighborFilter(std::string _unique_name, const paramsServer& _server):
- ProcessorParamsLoopClosure(_unique_name, _server)
- {
- buffer_size_ = _server.getParam<int>(_unique_name + "/buffer_size");
- sample_step_degree_ = _server.getParam<int>(_unique_name + "/sample_step_degree");
- auto distance_type_str = _server.getParam<std::string>(_unique_name + "/distance_type");
- if(distance_type_str.compare("LC_POINT_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSE;
- else if(distance_type_str.compare("LC_ELLIPSE_ELLIPSE")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
- else if(distance_type_str.compare("LC_POINT_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_POINT_ELLIPSOID;
- else if(distance_type_str.compare("LC_ELLIPSOID_ELLIPSOID")) distance_type_ = LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID;
- else if(distance_type_str.compare("LC_MAHALANOBIS_DISTANCE")) distance_type_ = LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE;
- else throw std::runtime_error("Failed to fetch a valid value for the enumerate LoopclosureDistanceType. Value provided: " + distance_type_str);
- probability_ = _server.getParam<Scalar>(_unique_name + "/probability");
- }
- virtual ~ProcessorParamsFrameNearestNeighborFilter() = default;
-
- int buffer_size_;
- int sample_step_degree_;
- DistanceType distance_type_;
- Scalar probability_;
-};
-
-class ProcessorFrameNearestNeighborFilter : public ProcessorLoopClosureBase
-{
-private:
-
- // area of the computed covariance ellipse.
- // depends on how many percent of data should be considered.
- Scalar area_;
-
- ProcessorParamsFrameNearestNeighborFilterPtr params_NNF;
-
-public:
-
- using Params = ProcessorParamsFrameNearestNeighborFilter;
- using ParamsPtr = ProcessorParamsFrameNearestNeighborFilterPtr;
- using DistanceType = Params::DistanceType;
-
- ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF);
- virtual ~ProcessorFrameNearestNeighborFilter() = default;
- virtual void configure(SensorBasePtr _sensor) { };
-
- inline DistanceType getDistanceType() const noexcept {return params_NNF->distance_type_;}
-
-protected:
-
- virtual bool findCandidates(const CaptureBasePtr& _incoming_ptr);
-
- // returns Ellipse in 2D case [ pos_x, pos_y, a, b, tilt]
- bool computeEllipse2D(const FrameBasePtr& frame_ptr,
- Eigen::Vector5s& ellipse);
-
- // returns Ellipsoid in 3D case
- // [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
- bool computeEllipsoid3D(const FrameBasePtr& frame_ptr,
- Eigen::Vector10s& ellipsoid);
-
- // returns true if the two 2D ellipses intersect
- bool ellipse2DIntersect(const Eigen::VectorXs &ellipse1,
- const Eigen::VectorXs &ellipse2);
-
- // returns true if a 2D point lies inside a 2D ellipse
- bool point2DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipse);
-
- // returns true if the two 3D ellipsoids intersect
- bool ellipsoid3DIntersect(const Eigen::VectorXs &ellipsoid1,
- const Eigen::VectorXs &ellipsoid2);
-
- // returns true if a 3D point lies inside a 3D ellipsoid
- bool point3DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipsoid);
-
- // returns true if frame lies within Mahalanobis Distance
- bool insideMahalanisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame);
-
- // computes the Mahalanobis Distance
- Scalar MahalanobisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame);
-
- bool frameInsideBuffer(const FrameBasePtr& frame_ptr);
-};
-
-} // namespace wolf
-
-#endif // _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H_
diff --git a/include/core/processor/processor_loopclosure_base.h b/include/core/processor/processor_loopclosure_base.h
deleted file mode 100644
index a5dc8589156b6b525b0a9b551ec3dc55ab7f5af7..0000000000000000000000000000000000000000
--- a/include/core/processor/processor_loopclosure_base.h
+++ /dev/null
@@ -1,140 +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(ProcessorParamsLoopClosure);
-
-struct ProcessorParamsLoopClosure : public ProcessorParamsBase
-{
- using ProcessorParamsBase::ProcessorParamsBase;
-// virtual ~ProcessorParamsLoopClosure() = default;
-
- // add neccesery parameters for loop closure initialisation here and initialize
- // them in constructor
-};
-
-/** \brief General loop closure processor
- *
- * This is an abstract class.
- *
- * 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 ProcessorLoopClosureBase : public ProcessorBase
-{
-protected:
-
- ProcessorParamsLoopClosurePtr params_loop_closure_;
-
- // Frames that are possible loop closure candidates according to
- // findLoopClosure()
- std::vector<FrameBasePtr> loop_closure_candidates;
-
- // Frames that are possible loop closure candidates according to
- // findLoopClosure(), but are too recent in the timeline, aka still in a
- // 'buffer zone'. This vector will capture the frames that were set just before
- // the last keyframe.
- std::vector<FrameBasePtr> close_candidates;
-
-public:
-
- ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure);
-
- virtual ~ProcessorLoopClosureBase() = default;
- virtual void configure(SensorBasePtr _sensor) override { };
-
- /** \brief Full processing of an incoming Capture.
- *
- * Usually you do not need to overload this method in derived classes.
- * Overload it only if you want to alter this algorithm.
- */
- virtual void process(CaptureBasePtr _incoming_ptr) override;
-
- virtual void keyFrameCallback(FrameBasePtr _keyframe_ptr,
- const Scalar& _time_tol_other) override ;
-
- const std::vector<FrameBasePtr>& getCandidates() const noexcept;
-
- const std::vector<FrameBasePtr>& getCloseCandidates() const noexcept;
-
-protected:
-
- /** 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() { }
-
- /** Find a loop closure between incoming data and all keyframe data
- *
- * This is called by process() .
- *
- * Overload this function in derived classes to find loop closure.
- */
- virtual bool findCandidates(const CaptureBasePtr& _incoming_ptr) = 0;
-
- /** perform a validation among the found possible loop closures to confirm
- * or dismiss them based on available data
- *
- * This is called by process() .
- *
- * Overload this function in derived classes to confirm loop closure.
- */
- virtual bool confirmLoopClosure() = 0;
-
- /** \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!
- */
- virtual bool voteForKeyFrame() override = 0;
-};
-
-inline const std::vector<FrameBasePtr>&
-ProcessorLoopClosureBase::getCandidates() const noexcept
-{
- return loop_closure_candidates;
-}
-
-inline const std::vector<FrameBasePtr>&
-ProcessorLoopClosureBase::getCloseCandidates() const noexcept
-{
- return close_candidates;
-}
-
-} // namespace wolf
-
-#endif /* _WOLF_PROCESSOR_LOOPCLOSURE_BASE_H */
diff --git a/src/processor/processor_frame_nearest_neighbor_filter.cpp b/src/processor/processor_frame_nearest_neighbor_filter.cpp
deleted file mode 100644
index b4f6303b128b37ba74104cddf02dd82e328c08e8..0000000000000000000000000000000000000000
--- a/src/processor/processor_frame_nearest_neighbor_filter.cpp
+++ /dev/null
@@ -1,506 +0,0 @@
-#include "core/processor/processor_frame_nearest_neighbor_filter.h"
-
-namespace wolf
-{
-ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(ParamsPtr _params_NNF):
- ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER", _params_NNF),
- params_NNF(_params_NNF)
-{
- // area of ellipse based on the Chi-Square Probabilities
- // https://people.richland.edu/james/lecture/m170/tbl-chi.html
- // cover both 2D and 3D cases
-
- if(params_NNF->distance_type_ == DistanceType::LC_POINT_ELLIPSE ||
- params_NNF->distance_type_ == DistanceType::LC_ELLIPSE_ELLIPSE)
- {
- switch ((int)(params_NNF->probability_*100))
- {
- case 900: area_ = 4.605; // 90% probability
- break;
- case 950: area_ = 5.991; // 95% probability
- break;
- case 990: area_ = 9.210; // 99% probability
- break;
- default : area_ = 5.991; // 95% probability
- break;
- }
- }
- if (params_NNF->distance_type_ == DistanceType::LC_POINT_ELLIPSOID ||
- params_NNF->distance_type_ == DistanceType::LC_ELLIPSOID_ELLIPSOID)
- {
- switch ((int)(params_NNF->probability_*100))
- {
- case 900: area_ = 6.251; // 90% probability
- break;
- case 950: area_ = 7.815; // 95% probability
- break;
- case 990: area_ = 11.345; // 99% probability
- break;
- default : area_ = 7.815; // 95% probability
- break;
- }
- }
-
- /*
- * The area is based on the Chi-Square Probabilities
- * Becasue they are very big for high likelihood, it is here manually set
- * on ONE. Therefore the ellipses/ ellipsoids are based on the unit ellipse/
- * ellipsoids and the axis are scaled by a / b / c
- */
-
- area_ = 1;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::findCandidates(const CaptureBasePtr& /*_incoming_ptr*/)
-{
- const ProblemPtr problem_ptr = getProblem();
-
- const std::string frame_structure =
- problem_ptr->getTrajectory()->getFrameStructure();
-
- // get the list of all frames
- const FrameBasePtrList& frame_list = problem_ptr->
- getTrajectory()->
- getFrameList();
-
- bool found_possible_candidate = false;
-
- for (const FrameBasePtr& key_it : frame_list)
- {
- // check for LC just if frame is key frame
- // Assert that the evaluated KF has a capture of the
- // same sensor as this processor
- if (key_it->isKey() && key_it->getCaptureOf(getSensor()/*, "LASER 2D"*/) != nullptr)
- {
- // Check if the two frames currently evaluated are already
- // constrained one-another.
- const FactorBasePtrList& fac_list = key_it->getConstrainedByList();
-
- bool are_constrained = false;
- for (const FactorBasePtr& crt : fac_list)
- {
- // Are the two frames constrained one-another ?
- if (crt->getFrameOther() == problem_ptr->getLastKeyFrame())
- {
- // By this very processor ?
- if (crt->getProcessor() == shared_from_this())
- {
- WOLF_DEBUG("Frames are already constrained together.");
- are_constrained = true;
- break;
- }
- }
- }
- if (are_constrained) continue;
-
- // check if feame is potential candidate for loop closure with
- // chosen distance type
- switch (params_NNF->distance_type_)
- {
- case LoopclosureDistanceType::LC_POINT_ELLIPSE:
- {
- if (frame_structure == "PO 3D" || frame_structure == "POV 3D") // @todo add frame structure "PQVBB 3D"
- {
- WOLF_ERROR("Distance Type LC_POINT_ELLIPSE does not fit 3D frame structure");
- return false;
- }
-
- Eigen::Vector5s frame_covariance;
- if (!computeEllipse2D(key_it, frame_covariance)) continue;
-
- const Eigen::VectorXs current_position = getProblem()->getCurrentState();
- found_possible_candidate = point2DIntersect(current_position,
- frame_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE:
- {
- if (frame_structure == "PO 3D" || frame_structure == "POV 3D")
- {
- WOLF_ERROR("Distance Type LC_ELLIPSE_ELLIPSE does not fit 3D frame structure");
- return false;
- }
-
- Eigen::Vector5s frame_covariance, current_covariance;
- if (!computeEllipse2D(key_it,
- frame_covariance)) continue;
- if (!computeEllipse2D(getProblem()->getLastKeyFrame(),
- current_covariance)) continue;
- found_possible_candidate = ellipse2DIntersect(frame_covariance,
- current_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_POINT_ELLIPSOID:
- {
- if (frame_structure == "PO 2D")
- {
- WOLF_ERROR("Distance Type POINT_ELLIPSOID does not fit 2D frame structure");
- return false;
- }
- Eigen::Vector10s frame_covariance;
- if (!computeEllipsoid3D(key_it,
- frame_covariance)) continue;
- const Eigen::VectorXs current_position = getProblem()->getCurrentState();
- found_possible_candidate = point3DIntersect(current_position,
- frame_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_ELLIPSOID_ELLIPSOID:
- {
- if (frame_structure == "PO 2D")
- {
- WOLF_ERROR("Distance Type ELLIPSOID_ELLIPSOID does not fit 2D frame structure");
- return false;
- }
- Eigen::Vector10s frame_covariance, current_covariance;
- if (!computeEllipsoid3D(key_it,
- frame_covariance)) continue;
- if (!computeEllipsoid3D(getProblem()->getLastKeyFrame(),
- frame_covariance)) continue;
- found_possible_candidate = ellipsoid3DIntersect(frame_covariance,
- current_covariance);
- break;
- }
-
- case LoopclosureDistanceType::LC_MAHALANOBIS_DISTANCE:
- {
- found_possible_candidate = insideMahalanisDistance(key_it,
- problem_ptr->getLastKeyFrame());
- break;
- }
-
- default:
- WOLF_WARN("NO IMPLEMENTATION FOR OTHER DISTANCE CALCULATION TYPES YET.");
- found_possible_candidate = false;
- return false;
- }
-
- // if a candidate was detected, push it to the appropiate list
- if (found_possible_candidate)
- {
- if (!frameInsideBuffer(key_it))
- loop_closure_candidates.push_back(key_it);
- else
- close_candidates.push_back(key_it);
- }
-
- } // end if key_it is keyframe
- } // end loop through every frame in list
-
- return found_possible_candidate;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::computeEllipse2D(const FrameBasePtr& frame_ptr,
- Eigen::Vector5s& ellipse)
-{
- // get 3x3 covariance matrix AKA variance
- Eigen::MatrixXs covariance;
- if (!getProblem()->getFrameCovariance(frame_ptr, covariance))
- {
- WOLF_WARN("Frame covariance not found!");
- return false;
- }
-
- if (!isCovariance(covariance))
- {
- WOLF_WARN("Covariance is not proper !");
- return false;
- }
-
- // get position of frame AKA mean [x, y]
- const Eigen::VectorXs frame_position = frame_ptr->getP()->getState();
- ellipse(0) = frame_position(0);
- ellipse(1) = frame_position(1);
-
- // compute covariance error ellipse around state
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix2s> solver(covariance.block(0,0,2,2));
- Scalar eigenvalue1 = solver.eigenvalues()[0]; // smaller value
- Scalar eigenvalue2 = solver.eigenvalues()[1]; // bigger value
- //Eigen::VectorXs eigenvector1 = solver.eigenvectors().col(0);
- Eigen::VectorXs eigenvector2 = solver.eigenvectors().col(1);
-
- const Scalar scale = std::sqrt(area_);
- ellipse(2) = scale * std::sqrt(eigenvalue2) / 2.; // majorAxis
- ellipse(3) = scale * std::sqrt(eigenvalue1) / 2.; // minorAxis
- ellipse(4) = std::atan2(eigenvector2[1], eigenvector2[0]); // tilt
-
- if (ellipse(4) < Scalar(0)) ellipse(4) += Scalar(2) * M_PI;
-
- // [ pos_x, pos_y, a, b, tilt]
- return true;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::computeEllipsoid3D(const FrameBasePtr& frame_pointer,
- Eigen::Vector10s& ellipsoid)
-{
- // Ellipse description [ pos_x, pos_y, pos_z, a, b, c, quat_w, quat_z, quat_y, quat_z]
-
- // get position of frame AKA mean [x, y, z]
- const Eigen::VectorXs frame_position = frame_pointer->getP()->getState();
- ellipsoid(0) = frame_position(0);
- ellipsoid(1) = frame_position(1);
- ellipsoid(2) = frame_position(2);
-
- // get 9x9 covariance matrix AKA variance
- Eigen::MatrixXs covariance;
- if (!getProblem()->getFrameCovariance(frame_pointer, covariance))
- {
- WOLF_WARN("Frame covariance not found!");
- return false;
- }
-
- if (!isCovariance(covariance))
- {
- WOLF_WARN("Covariance is not proper !");
- return false;
- }
-
- Eigen::SelfAdjointEigenSolver<Eigen::Matrix3s> solver(covariance.block(0,0,3,3));
- const Scalar eigenvalue1 = solver.eigenvalues()[0]; // smaller value
- const Scalar eigenvalue2 = solver.eigenvalues()[1]; // mediate value
- const Scalar eigenvalue3 = solver.eigenvalues()[2]; // bigger value
-
- const Scalar scale = std::sqrt(area_);
-
- ellipsoid(3) = scale * std::sqrt(std::abs(eigenvalue3)) / Scalar(2); // majorAxis
- ellipsoid(4) = scale * std::sqrt(std::abs(eigenvalue2)) / Scalar(2); // mediumAxis
- ellipsoid(5) = scale * std::sqrt(std::abs(eigenvalue1)) / Scalar(2); // minorAxis
-
- // ROTATION COMPUTATION get rotation of the three axis from eigenvector
- // eigenvector belonging to biggest eigenvalue gives direction / z_axis
- // of ellipsoid
-
- // get eigenvectors that correspond to eigenvalues
- //const Eigen::Vector3s eigenvector1 = solver.eigenvectors().col(0); // minorAxis
- const Eigen::Vector3s eigenvector2 = solver.eigenvectors().col(1); // mediumAxis ->y
- const Eigen::Vector3s eigenvector3 = solver.eigenvectors().col(2); // majorAxis -> x
-
- // computed axis of coordinate system in ellipsoid center
- const Eigen::Vector3s z_axis = eigenvector3.cross(eigenvector2);
- const Eigen::Vector3s y_axis = z_axis.cross(eigenvector3);
-
- // use them to fill rotation matrix
- Eigen::Matrix3s rotationMatrix;
- rotationMatrix.col(0) = eigenvector3.normalized();
- rotationMatrix.col(1) = y_axis.normalized();
- rotationMatrix.col(2) = z_axis.normalized();
-
- // get quaternions for transformation
- Eigen::Quaternions rotation(rotationMatrix);
- rotation.normalize();
-
- ellipsoid(6) = rotation.w();
- ellipsoid(7) = rotation.x();
- ellipsoid(8) = rotation.y();
- ellipsoid(9) = rotation.z();
-
- WOLF_DEBUG("made ellipsoid: \n[", ellipsoid.transpose(), "]");
-
- // [ pos_x, pos_y, pos_z, a, b, c, alpha, beta, gamma]
- return true;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::ellipse2DIntersect(const Eigen::VectorXs& ellipse1,
- const Eigen::VectorXs& ellipse2)
-{
- for (int i = 0 ; i < 360 ; i += params_NNF->sample_step_degree_)
- {
- // parameterized form of first ellipse gives point on first ellipse
- // https://www.uwgb.edu/dutchs/Geometry/HTMLCanvas/ObliqueEllipses5.HTM
- Eigen::VectorXs pointOnEllipse(2);
- const Scalar theta = Scalar(i) * M_PI / 180.0;
-
- pointOnEllipse(0) = ellipse1(0) +
- ellipse1(2) * std::cos(ellipse1(4)) * std::cos(theta) -
- ellipse1(3) * std::sin(ellipse1(4)) * std::sin(theta);
-
- pointOnEllipse(1) = ellipse1(1) +
- ellipse1(2) * std::sin(ellipse1(4)) * std::cos(theta) +
- ellipse1(3) * std::cos(ellipse1(4)) * std::sin(theta);
-
- //WOLF_DEBUG(" for ", i, " deg / ", theta " rad ---> [" ,
- // pointOnEllipse.transpose(), "]");
-
- // check if point lies inside the other ellipse
- if (point2DIntersect(pointOnEllipse, ellipse2)) return true;
- }
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::point2DIntersect(const Eigen::VectorXs& point,
- const Eigen::VectorXs& ellipse)
-{
- const Scalar area51 = std::pow( ((point(0) - ellipse(0)) * cos(ellipse(4))
- + (point(1) - ellipse(1)) * sin(ellipse(4))), 2 )
- / std::pow( ellipse(2), 2 )
- + std::pow( ((point(0) - ellipse(0)) * sin(ellipse(4))
- - (point(1) - ellipse(1)) * cos(ellipse(4))), 2 )
- / std::pow( ellipse(3), 2 );
-
- //WOLF_DEBUG("computed area = ", area51);
-
- if ( area51 - area_ <= 0) return true;
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::ellipsoid3DIntersect(const Eigen::VectorXs &ellipsoid1,
- const Eigen::VectorXs &ellipsoid2)
-{
- // get transformation from elliposiod1 center frame to world frame
- Eigen::Matrix4s transformation_matrix;
- Eigen::Quaternions rotation(ellipsoid1(6),ellipsoid1(7),
- ellipsoid1(8),ellipsoid1(9));
-// Eigen::Vector4s translation;
-// translation << ellipsoid1(0), ellipsoid1(1), ellipsoid1(2), 1;
- transformation_matrix.block(0,0,3,3) = rotation.toRotationMatrix();
- transformation_matrix.col(3) << ellipsoid1(0), ellipsoid1(1), ellipsoid1(2), 1;
-
- Eigen::Vector4s point_hom = Eigen::Vector4s::Constant(1);
-
- Scalar omega = 0;
- for (int i = 0 ; i < 360 ; i += params_NNF->sample_step_degree_)
- {
- const Scalar theta = Scalar(i) * M_PI / 180.0;
- for( int j = 0 ; j < 180 ; j += params_NNF->sample_step_degree_)
- {
- omega = Scalar(j) * M_PI / 180.0;
-
- // compute point on surface of first ellipsoid
- point_hom(0) = ellipsoid1(3) * std::cos(theta) * std::sin(omega);
- point_hom(1) = ellipsoid1(4) * std::sin(theta) * std::sin(omega);
- point_hom(2) = ellipsoid1(5) * std::cos(omega);
-
- // transform point into world frame
- const Eigen::Vector4s point_on_ellipsoid = transformation_matrix * point_hom;
-
- // check if 3D point lies inside the second ellipsoid
- if (point3DIntersect(point_on_ellipsoid, ellipsoid2))
- return true;
- }
- }
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::point3DIntersect(const Eigen::VectorXs &point,
- const Eigen::VectorXs &ellipsoid)
-{
- // get transformation from ellipsoid center frame to world frame
- Eigen::Matrix4s transformation_matrix = Eigen::Matrix4s::Identity();
-
- Eigen::Quaternions rotation(ellipsoid(6),ellipsoid(7),
- ellipsoid(8),ellipsoid(9));
-// Eigen::Vector4s translation;
-// translation << ellipsoid(0), ellipsoid(1), ellipsoid(2), 1;
- transformation_matrix.block(0,0,3,3) = rotation.toRotationMatrix();
- transformation_matrix.col(3) << ellipsoid(0), ellipsoid(1), ellipsoid(2), 1;
- // inverse to get transformation from world frame to ellipsoid center frame
- transformation_matrix = transformation_matrix.inverse().eval();
-
- // homogenize 3D point
- // ???
- Eigen::Vector4s point_hom;
- point_hom << point(0), point(1), point(2), 1;
-
- // transform point from world frame to elliposiod center frame
- Eigen::Vector4s transformed_point = transformation_matrix * point_hom;
-
- // check if point is inside ellipsoid with general equation
- Scalar area51 = std::pow(transformed_point(0), 2) / std::pow( ellipsoid(3), 2) +
- std::pow(transformed_point(1), 2) / std::pow(ellipsoid(4), 2) +
- std::pow(transformed_point(2), 2) / std::pow(ellipsoid(5), 2);
-
- WOLF_DEBUG("computed area = ", area51);
-
- if (area51 - area_ <= 0) return true;
-
- return false;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::insideMahalanisDistance(const FrameBasePtr& trajectory_frame,
- const FrameBasePtr& query_frame)
-{
- const Scalar distance = MahalanobisDistance(trajectory_frame, query_frame);
-
- WOLF_DEBUG("Mahalanobis Distance = ", distance);
-
- if ( distance < 0 )
- {
- WOLF_DEBUG("NO COVARIANCE AVAILABLE");
- }
-
- /*
- * TODO:
- * check if distance is smaller than a threshold,
- * if yes -> return true
- * else -> return false
- *
- */
- return false;
-}
-
-//##############################################################################
-Scalar ProcessorFrameNearestNeighborFilter::MahalanobisDistance(const FrameBasePtr& trajectory,
- const FrameBasePtr& query)
-{
- Scalar distance = -1;
- Eigen::VectorXs traj_pose, query_pose;
-
- // get state and covariance matrix for both frames
- if (trajectory->getP()->getState().size() == 2)
- {
- traj_pose.resize(3);
- query_pose.resize(3);
- }
- else
- {
- traj_pose.resize(7);
- query_pose.resize(7);
- }
-
- traj_pose << trajectory->getP()->getState(),
- trajectory->getO()->getState();
-
- query_pose << query->getP()->getState(),
- query->getO()->getState();
-
- Eigen::MatrixXs traj_covariance, query_covariance;
- if ( !getProblem()->getFrameCovariance(trajectory, traj_covariance) ||
- !getProblem()->getFrameCovariance(query, query_covariance) ||
- !isCovariance(traj_covariance) ||
- !isCovariance(query_covariance))
- return distance;
-
- const Eigen::MatrixXs covariance = traj_covariance * query_covariance.transpose();
-
- const Eigen::VectorXs pose_difference = traj_pose - query_pose;
- distance = pose_difference.transpose() * covariance * pose_difference;
- distance = std::sqrt(distance);
-
- return distance;
-}
-
-//##############################################################################
-bool ProcessorFrameNearestNeighborFilter::frameInsideBuffer(const FrameBasePtr& frame_ptr)
-{
- FrameBasePtr keyframe = getProblem()->getLastKeyFrame();
- if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_NNF->buffer_size_ ))
- return false;
- else
- return true;
-}
-
-} // namespace wolf
-
diff --git a/src/processor/processor_loopclosure_base.cpp b/src/processor/processor_loopclosure_base.cpp
deleted file mode 100644
index 7ce19414499ead4b38e8432077f47e15116ca27e..0000000000000000000000000000000000000000
--- a/src/processor/processor_loopclosure_base.cpp
+++ /dev/null
@@ -1,49 +0,0 @@
-/**
- * \file processor_loop_closure.h
- *
- * Created on: Aug 10, 2017
- * \author: Tessa Johanna
- */
-
-#include "core/processor/processor_loopclosure_base.h"
-
-namespace wolf
-{
-
-ProcessorLoopClosureBase::ProcessorLoopClosureBase(const std::string& _type, ProcessorParamsLoopClosurePtr _params_loop_closure):
- ProcessorBase(_type, _params_loop_closure),
- params_loop_closure_(_params_loop_closure)
-{
- //
-}
-
-//##############################################################################
-void ProcessorLoopClosureBase::process(CaptureBasePtr _incoming_ptr)
-{
- // clear all previous data from vector
- loop_closure_candidates.clear();
- close_candidates.clear();
-
- // the pre-process, if necessary, is implemented in the derived classes
- preProcess();
-
- findCandidates(_incoming_ptr);
-
- confirmLoopClosure();
-
- WOLF_DEBUG("ProcessorLoopClosureBase::process found ",
- loop_closure_candidates.size(), " candidates found.");
-
- // the post-process, if necessary, is implemented in the derived classes
- postProcess();
-}
-
-//##############################################################################
-void ProcessorLoopClosureBase::keyFrameCallback(FrameBasePtr /*_keyframe_ptr*/,
- const Scalar& /*_time_tol_other*/)
-{
- //
-}
-
-}// namespace wolf
-
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 2bdc605a30dc5f70f4028c6c5007f8b731b48467..0b98845f07bc3bf920e1ead358bb6b5d91be4e73 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -178,10 +178,6 @@ target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PROJECT_NAME})
-# ProcessorFrameNearestNeighborFilter class test
-wolf_add_gtest(gtest_processor_frame_nearest_neighbor_filter_2D gtest_processor_frame_nearest_neighbor_filter_2D.cpp)
-target_link_libraries(gtest_processor_frame_nearest_neighbor_filter_2D ${PROJECT_NAME})
-
# ProcessorMotion in 2D
wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
diff --git a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
deleted file mode 100644
index 82b3d2ae02e1e1821b9dca28b35792f6855a8f5e..0000000000000000000000000000000000000000
--- a/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp
+++ /dev/null
@@ -1,228 +0,0 @@
-
-/**
- * \file gtest_processor_frame_nearest_neighbor_filter.cpp
- *
- * Created on: Aug 2, 2017
- * \author: tessajohanna
- */
-
-#include "core/utils/utils_gtest.h"
-#include "core/utils/logging.h"
-
-#include "core/sensor/sensor_odom_2D.h"
-#include "core/processor/processor_frame_nearest_neighbor_filter.h"
-
-#include <iostream>
-
-// Dummy class so that it is no pur virtual
-struct DummyLoopCloser : public wolf::ProcessorFrameNearestNeighborFilter
-{
- DummyLoopCloser(ParamsPtr _params) :
- wolf::ProcessorFrameNearestNeighborFilter(_params) { }
-
- bool confirmLoopClosure() override { return false; }
- bool voteForKeyFrame() override { return false; }
-};
-
-// Declare Wolf problem
-wolf::ProblemPtr problem = wolf::Problem::create("PO", 2);
-
-// Declare Sensor
-Eigen::Vector3s odom_extrinsics = Eigen::Vector3s(0,0,0);
-
-std::shared_ptr<wolf::IntrinsicsOdom2D> odom_intrinsics =
- std::make_shared<wolf::IntrinsicsOdom2D>(wolf::IntrinsicsOdom2D());
-
-wolf::SensorBasePtr sensor_ptr;
-
-// Declare Processors
-wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_point2d;
-wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_point2d =
- std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>();
-
-wolf::ProcessorFrameNearestNeighborFilterPtr processor_ptr_ellipse2d;
-wolf::ProcessorParamsFrameNearestNeighborFilterPtr processor_params_ellipse2d =
- std::make_shared<wolf::ProcessorParamsFrameNearestNeighborFilter>();
-
-// Declare Frame Pointer
-wolf::StateBlockPtr stateblock_ptr1, stateblock_ptr2, stateblock_ptr3, stateblock_ptr4;
-wolf::FrameBasePtr F1, F2, F3, F4;
-wolf::CaptureBasePtr capture_dummy, incomming_dummy;
-
-//##############################################################################
-TEST(ProcessorFrameNearestNeighborFilter, PointInEllipseRotated)
-{
- // four different states [x, y]
- Eigen::Vector3s state1, state2, state3, state4;
- state1 << 3.0, 5.0, 0.0;
- state2 << 3.0, 6.0, 0.0;
- state3 << 3.0, 7.0, 0.0;
- state4 << 100.0, 100.0, 0.0;
-
-
- // create Keyframes
- F1 = problem->emplaceFrame(wolf::KEY, state1, 1);
- F2 = problem->emplaceFrame(wolf::KEY, state2, 2);
- F3 = problem->emplaceFrame(wolf::KEY, state3, 3);
- F4 = problem->emplaceFrame(wolf::KEY, state4, 4);
-
- auto stateblock_pptr1 = F1->getP();
- auto stateblock_optr1 = F1->getO();
-
- auto stateblock_pptr2 = F2->getP();
- auto stateblock_optr2 = F2->getO();
-
- auto stateblock_pptr3 = F3->getP();
- auto stateblock_optr3 = F3->getO();
-
- auto stateblock_pptr4 = F4->getP();
- auto stateblock_optr4 = F4->getO();
-
- // add dummy capture
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F1, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F2, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F3, "DUMMY", 1.0, sensor_ptr);
- wolf::CaptureBase::emplace<wolf::CaptureBase>(F4, "DUMMY", 1.0, sensor_ptr);
-
- // capture_dummy = std::make_shared<wolf::CaptureBase>("DUMMY",
- // 1.0,
- // sensor_ptr);
- // F1->addCapture(capture_dummy);
- // F2->addCapture(capture_dummy);
- // F3->addCapture(capture_dummy);
- // F4->addCapture(capture_dummy);
-
- // Add same covariances for all states
- Eigen::Matrix2s position_covariance_matrix;
- position_covariance_matrix << 9.0, 0.0,
- 0.0, 5.0;
-
- Eigen::Matrix1s orientation_covariance_matrix;
- orientation_covariance_matrix << 0.01;
-
- Eigen::Vector2s tt_covariance_matrix;
- tt_covariance_matrix << 0.0, 0.0;
-
- problem->addCovarianceBlock( stateblock_pptr1, stateblock_pptr1,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr1, stateblock_optr1,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr1, stateblock_optr1,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr2, stateblock_pptr2,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr2, stateblock_optr2,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr2, stateblock_optr2,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr3, stateblock_pptr3,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr3, stateblock_optr3,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr3, stateblock_optr3,
- tt_covariance_matrix);
-
- problem->addCovarianceBlock( stateblock_pptr4, stateblock_pptr4,
- position_covariance_matrix);
- problem->addCovarianceBlock( stateblock_optr4, stateblock_optr4,
- orientation_covariance_matrix);
- problem->addCovarianceBlock( stateblock_pptr4, stateblock_optr4,
- tt_covariance_matrix);
- // create dummy capture
- incomming_dummy = wolf::CaptureBase::emplace<wolf::CaptureBase>(nullptr, "DUMMY", 1.2, sensor_ptr);
-
- // Make 3rd frame last Key frame
- F3->setTimeStamp(wolf::TimeStamp(2.0));
- problem->getTrajectory()->sortFrame(F3);
-
- // trigger search for loopclosure
- processor_ptr_point2d->process(incomming_dummy);
-
- // const std::vector<wolf::FrameBasePtr> &testloops =
- // processor_ptr_point2d->getCandidates();
-
- //TODO: Due to changes in the emplace refactor these tests are not working. To be fixed.
- // ASSERT_EQ(testloops.size(), (unsigned int) 1);
- // ASSERT_EQ(testloops[0]->id(), (unsigned int) 2);
-}
-
-//##############################################################################
-TEST(ProcessorFrameNearestNeighborFilter, EllipseEllipseRotatedCrosscovariance)
-{
- // set covariance to a -90 degree turned ellipse
- Eigen::Matrix2s position_covariance_matrix;
-// position_covariance_matrix << 9.0, 1.8,
-// 1.8, 5.0;
-
- position_covariance_matrix << 5.0, 0.0,
- 0.0, 9.0;
-
- problem->addCovarianceBlock( F1->getP(), F1->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F2->getP(), F2->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F3->getP(), F3->getP(),
- position_covariance_matrix);
- problem->addCovarianceBlock( F4->getP(), F4->getP(),
- position_covariance_matrix);
-
- // Make 3rd frame last Key frame
- F3->setTimeStamp(wolf::TimeStamp(2.0));
- problem->getTrajectory()->sortFrame(F3);
-
- // trigger search for loopclosure
- processor_ptr_ellipse2d->process(incomming_dummy);
- const std::vector<wolf::FrameBasePtr> &testloops =
- processor_ptr_ellipse2d->getCandidates();
-
- //TODO: Due to changes in the emplace refactor these tests are not working. To be fixed.
- // ASSERT_EQ(testloops.size(), (unsigned int) 2);
- // ASSERT_EQ(testloops[0]->id(), (unsigned int) 1);
- // ASSERT_EQ(testloops[1]->id(), (unsigned int) 2);
-
- // Make 4th frame last Key frame
- F4->setTimeStamp(wolf::TimeStamp(3.0));
- problem->getTrajectory()->sortFrame(F4);
-
- // trigger search for loopclosure again
- processor_ptr_ellipse2d->process(incomming_dummy);
- ASSERT_EQ(testloops.size(), (unsigned int) 0);
-}
-
-//##############################################################################
-int main(int argc, char **argv)
-{
- // SENSOR PARAMETERS
- odom_intrinsics->k_disp_to_disp = 0.2;
- odom_intrinsics->k_rot_to_rot = 0.2;
-
- // install sensor
- sensor_ptr = problem->installSensor("ODOM 2D", "odom",
- odom_extrinsics, odom_intrinsics);
-
- // install the processors
- processor_params_point2d->probability_ = 0.95;
- processor_params_point2d->buffer_size_ = 0; // just exclude identical frameIDs
- processor_params_point2d->distance_type_ = wolf::LoopclosureDistanceType::LC_POINT_ELLIPSE;
-
- // processor_ptr_point2d = std::make_shared<DummyLoopCloser>(processor_params_point2d);
- processor_ptr_point2d = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, processor_params_point2d));
- processor_ptr_point2d->setName("processor2Dpoint");
-
- // sensor_ptr->addProcessor(processor_ptr_point2d);
-
- processor_params_ellipse2d->probability_ = 0.95;
- processor_params_ellipse2d->buffer_size_ = 0; // just exclude identical frameIDs
- processor_params_ellipse2d->distance_type_ = wolf::LoopclosureDistanceType::LC_ELLIPSE_ELLIPSE;
-
- // processor_ptr_ellipse2d = std::make_shared<DummyLoopCloser>(processor_params_ellipse2d);
- processor_ptr_ellipse2d = std::static_pointer_cast<wolf::ProcessorFrameNearestNeighborFilter>(wolf::ProcessorBase::emplace<DummyLoopCloser>(sensor_ptr, processor_params_ellipse2d));
- processor_ptr_ellipse2d->setName("processor2Dellipse");
-
- // sensor_ptr->addProcessor(processor_ptr_ellipse2d);
-
- testing::InitGoogleTest(&argc, argv);
- return RUN_ALL_TESTS();
-}