Skip to content
Snippets Groups Projects
Commit 1058f584 authored by PierreGtch's avatar PierreGtch
Browse files

Remove old processor_loopclosure_base and dependency (processor_frame_nearest_neighbor_filter)

parent adcf4120
No related branches found
No related tags found
1 merge request!290Resolve "ProcessorLoopClosureBase class"
Hide whitespace changes
Inline Side-by-side
Showing
with 0 additions and 1059 deletions
CMakeLists.txt
+ 0
− 4
View file @ 1058f584
...@@ -249,9 +249,7 @@ SET(HDRS_PROCESSOR ...@@ -249,9 +249,7 @@ SET(HDRS_PROCESSOR
include/core/processor/processor_capture_holder.h include/core/processor/processor_capture_holder.h
include/core/processor/processor_diff_drive.h include/core/processor/processor_diff_drive.h
include/core/processor/processor_factory.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_logging.h
include/core/processor/processor_loopclosure_base.h
include/core/processor/processor_loopclosure_base2.h include/core/processor/processor_loopclosure_base2.h
include/core/processor/processor_motion.h include/core/processor/processor_motion.h
include/core/processor/processor_odom_2D.h include/core/processor/processor_odom_2D.h
...@@ -346,8 +344,6 @@ SET(SRCS_PROCESSOR ...@@ -346,8 +344,6 @@ SET(SRCS_PROCESSOR
src/processor/processor_base.cpp src/processor/processor_base.cpp
src/processor/processor_capture_holder.cpp src/processor/processor_capture_holder.cpp
src/processor/processor_diff_drive.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_loopclosure_base2.cpp
src/processor/processor_motion.cpp src/processor/processor_motion.cpp
src/processor/processor_odom_2D.cpp src/processor/processor_odom_2D.cpp
......
include/core/processor/processor_frame_nearest_neighbor_filter.h deleted 100644 → 0
+ 0
− 128
View file @ adcf4120
#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_
include/core/processor/processor_loopclosure_base.h deleted 100644 → 0
+ 0
− 140
View file @ adcf4120
#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 */
src/processor/processor_frame_nearest_neighbor_filter.cpp deleted 100644 → 0
+ 0
− 506
View file @ adcf4120
#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
src/processor/processor_loopclosure_base.cpp deleted 100644 → 0
+ 0
− 49
View file @ adcf4120
/**
* \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
test/CMakeLists.txt
+ 0
− 4
View file @ 1058f584
...@@ -178,10 +178,6 @@ target_link_libraries(gtest_map_yaml ${PROJECT_NAME}) ...@@ -178,10 +178,6 @@ target_link_libraries(gtest_map_yaml ${PROJECT_NAME})
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp) wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PROJECT_NAME}) 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 # ProcessorMotion in 2D
wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp) wolf_add_gtest(gtest_odom_2D gtest_odom_2D.cpp)
target_link_libraries(gtest_odom_2D ${PROJECT_NAME}) target_link_libraries(gtest_odom_2D ${PROJECT_NAME})
......
test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp deleted 100644 → 0
+ 0
− 228
View file @ adcf4120
/**
* \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();
}
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment