diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 335e51fb3c16cd8fc083ceb5e2794eb1ee034343..3d3536f6c7eefa65e3a0f8176719c9de2727800f 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -251,6 +251,7 @@ SET(HDRS landmark_line_2D.h landmark_polyline_2D.h local_parametrization_polyline_extreme.h + processor_frame_nearest_neighbor_filter.h processor_image_params.h processor_imu.h processor_imu_UnitTester.h @@ -321,6 +322,7 @@ SET(SRCS landmark_line_2D.cpp landmark_polyline_2D.cpp local_parametrization_polyline_extreme.cpp + processor_frame_nearest_neighbor_filter.cpp processor_imu.cpp processor_imu_UnitTester.cpp processor_odom_2D.cpp diff --git a/src/processor_frame_nearest_neighbor_filter.cpp b/src/processor_frame_nearest_neighbor_filter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..a9c1205653bd6f19c55c85c49058fdb75be22b9d --- /dev/null +++ b/src/processor_frame_nearest_neighbor_filter.cpp @@ -0,0 +1,493 @@ +#include "processor_frame_nearest_neighbor_filter.h" + +namespace wolf +{ + +ProcessorFrameNearestNeighborFilter::ProcessorFrameNearestNeighborFilter(const Params& _params): + ProcessorLoopClosureBase("FRAME NEAREST NEIGHBOR FILTER"), + params_(_params) +{ + // 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_.distance_type_ == DistanceType::LC_POINT_ELLIPSE || + params_.distance_type_ == DistanceType::LC_ELLIPSE_ELLIPSE) + { + switch ((int)(params_.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_.distance_type_ == DistanceType::LC_POINT_ELLIPSOID || + params_.distance_type_ == DistanceType::LC_ELLIPSOID_ELLIPSOID) + { + switch ((int)(params_.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->getTrajectoryPtr()->getFrameStructure(); + + // get the list of all frames + const FrameBaseList& frame_list = problem_ptr-> + getTrajectoryPtr()-> + 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(getSensorPtr()/*, "LASER 2D"*/) != nullptr) + { + // Check if the two frames currently evaluated are already + // constrained one-another. + const ConstraintBaseList& ctr_list = key_it->getConstrainedByList(); + + bool are_constrained = false; + for (const ConstraintBasePtr& crt : ctr_list) + { + // Are the two frames constrained one-another ? + if (crt->getFrameOtherPtr() == problem_ptr->getLastKeyFramePtr()) + { + // 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_.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()->getLastKeyFramePtr(), + 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()->getLastKeyFramePtr(), + 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->getLastKeyFramePtr()); + 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 + const Eigen::MatrixXs covariance = + getProblem()->getFrameCovariance(frame_ptr); + + 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->getPPtr()->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->getPPtr()->getState(); + ellipsoid(0) = frame_position(0); + ellipsoid(1) = frame_position(1); + ellipsoid(2) = frame_position(2); + + // get 9x9 covariance matrix AKA variance + const Eigen::MatrixXs covariance = getProblem()->getFrameCovariance(frame_pointer); + + if (!isCovariance(covariance)) 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_.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_.sample_step_degree_) + { + const Scalar theta = Scalar(i) * M_PI / 180.0; + for( int j = 0 ; j < 180 ; j += params_.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->getPPtr()->getState().size() == 2) + { + traj_pose.resize(3); + query_pose.resize(3); + } + else + { + traj_pose.resize(7); + query_pose.resize(7); + } + + traj_pose << trajectory->getPPtr()->getState(), + trajectory->getOPtr()->getState(); + + query_pose << query->getPPtr()->getState(), + query->getOPtr()->getState(); + + const Eigen::MatrixXs traj_covariance = getProblem()->getFrameCovariance(trajectory); + const Eigen::MatrixXs query_covariance = getProblem()->getFrameCovariance(query); + + if ( !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()->getLastKeyFramePtr(); + if ( (int)frame_ptr->id() < ( (int)keyframe->id() - params_.buffer_size_ )) + return false; + else + return true; +} + +} // namespace wolf + diff --git a/src/processor_frame_nearest_neighbor_filter.h b/src/processor_frame_nearest_neighbor_filter.h new file mode 100644 index 0000000000000000000000000000000000000000..16c3985e035ce795e116fadec3e2ae1a83e224da --- /dev/null +++ b/src/processor_frame_nearest_neighbor_filter.h @@ -0,0 +1,114 @@ +#ifndef _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H +#define _WOLF_SRC_PROCESSOR_FRAME_NEAREST_NEIGHBOR_FILTER_H + +// Wolf related headers +#include "processor_loopclosure_base.h" +#include "state_block.h" + +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_) + { + // + } + + 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. + wolf::Scalar area_; + + ProcessorParamsFrameNearestNeighborFilter params_; + +public: + + using Params = ProcessorParamsFrameNearestNeighborFilter; + using ParamsPtr = ProcessorParamsFrameNearestNeighborFilterPtr; + using DistanceType = Params::DistanceType; + + ProcessorFrameNearestNeighborFilter(const Params& _params); + + virtual ~ProcessorFrameNearestNeighborFilter() = default; + + inline DistanceType getDistanceType() const noexcept {return params_.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/src/test/CMakeLists.txt b/src/test/CMakeLists.txt index 3763631a09f9297e2c7758383fe92d93ef9aaf33..8bd7628d1bf8b7fc4964d53e143b0672c6b5396b 100644 --- a/src/test/CMakeLists.txt +++ b/src/test/CMakeLists.txt @@ -114,6 +114,10 @@ target_link_libraries(gtest_frame_imu ${PROJECT_NAME}) wolf_add_gtest(gtest_pinhole gtest_pinhole.cpp) target_link_libraries(gtest_pinhole ${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}) + # ProcessorIMU class test wolf_add_gtest(gtest_processor_imu gtest_processor_imu.cpp) target_link_libraries(gtest_processor_imu ${PROJECT_NAME}) @@ -146,4 +150,4 @@ IF(OpenCV_FOUND) wolf_add_gtest(gtest_roi_ORB gtest_roi_ORB.cpp) target_link_libraries(gtest_roi_ORB ${PROJECT_NAME}) -ENDIF(OpenCV_FOUND) \ No newline at end of file +ENDIF(OpenCV_FOUND) diff --git a/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp b/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp new file mode 100644 index 0000000000000000000000000000000000000000..47c79e0f137b1e2911575b0c74b640fb03e3c896 --- /dev/null +++ b/src/test/gtest_processor_frame_nearest_neighbor_filter_2D.cpp @@ -0,0 +1,236 @@ + +/** + * \file gtest_processor_frame_nearest_neighbor_filter.cpp + * + * Created on: Aug 2, 2017 + * \author: tessajohanna + */ + + +#include "utils_gtest.h" +#include "../logging.h" + +#include "../sensor_odom_2D.h" +#include "../processor_frame_nearest_neighbor_filter.h" + +#include <iostream> + +// Dummy class so that it is no pur virtual +struct DummyLoopCloser : public wolf::ProcessorFrameNearestNeighborFilter +{ + DummyLoopCloser(const Params& _params) : + wolf::ProcessorFrameNearestNeighborFilter(_params) { } + + bool confirmLoopClosure() override { return false; } + bool voteForKeyFrame() override { return false; } +}; + +// Declare Wolf problem +wolf::ProblemPtr problem = wolf::Problem::create("PO 2D"); + +// 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; + + auto stateblock_pptr1 = std::make_shared<wolf::StateBlock>(state1.head<2>()); + auto stateblock_optr1 = std::make_shared<wolf::StateBlock>(state1.tail<1>()); + + auto stateblock_pptr2 = std::make_shared<wolf::StateBlock>(state2.head<2>()); + auto stateblock_optr2 = std::make_shared<wolf::StateBlock>(state2.tail<1>()); + + auto stateblock_pptr3 = std::make_shared<wolf::StateBlock>(state3.head<2>()); + auto stateblock_optr3 = std::make_shared<wolf::StateBlock>(state3.tail<1>()); + + auto stateblock_pptr4 = std::make_shared<wolf::StateBlock>(state4.head<2>()); + auto stateblock_optr4 = std::make_shared<wolf::StateBlock>(state4.tail<1>()); + + // create Keyframes + F1 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + 1, + stateblock_pptr1, + stateblock_optr1); + F2 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + 1, + stateblock_pptr2, + stateblock_optr2); + F3 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + 1, + stateblock_pptr3, + stateblock_optr3); + F4 = std::make_shared<wolf::FrameBase>(wolf::KEY_FRAME, + 1, + stateblock_pptr4, + stateblock_optr4); + + // add dummy capture + 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 first three states to trajectory + problem->getTrajectoryPtr()->addFrame(F1); + problem->getTrajectoryPtr()->addFrame(F2); + problem->getTrajectoryPtr()->addFrame(F3); + problem->getTrajectoryPtr()->addFrame(F4); + + // 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.0; + + 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 = std::make_shared<wolf::CaptureBase>("DUMMY", + 1.2, + sensor_ptr); + // Make 3rd frame last Keyframe + problem->getTrajectoryPtr()->setLastKeyFramePtr(F3); + + // trigger search for loopclosure + processor_ptr_point2d->process(incomming_dummy); + + const std::vector<wolf::FrameBasePtr> &testloops = + processor_ptr_point2d->getCandidates(); + + ASSERT_EQ(testloops.size(), 1); + ASSERT_EQ(testloops[0]->id(), 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->getPPtr(), F1->getPPtr(), + position_covariance_matrix); + problem->addCovarianceBlock( F2->getPPtr(), F2->getPPtr(), + position_covariance_matrix); + problem->addCovarianceBlock( F3->getPPtr(), F3->getPPtr(), + position_covariance_matrix); + problem->addCovarianceBlock( F4->getPPtr(), F4->getPPtr(), + position_covariance_matrix); + + // Make 3rd frame last Keyframe + problem->getTrajectoryPtr()->setLastKeyFramePtr(F3); + + // trigger search for loopclosure + processor_ptr_ellipse2d->process(incomming_dummy); + const std::vector<wolf::FrameBasePtr> &testloops = + processor_ptr_ellipse2d->getCandidates(); + + ASSERT_EQ(testloops.size(), 2); + ASSERT_EQ(testloops[0]->id(), 1); + ASSERT_EQ(testloops[1]->id(), 2); + + // Make 4th frame last Keyframe + problem->getTrajectoryPtr()->setLastKeyFramePtr(F4); + + // trigger search for loopclosure again + processor_ptr_ellipse2d->process(incomming_dummy); + ASSERT_EQ(testloops.size(), 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->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->setName("processor2Dellipse"); + + sensor_ptr->addProcessor(processor_ptr_ellipse2d); + + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +}