From aed1fbf5ee288de3fb7047393decb2b00a5648d0 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Joan=20Vallv=C3=A9=20Navarro?= <jvallve@iri.upc.edu> Date: Wed, 3 Apr 2019 14:27:55 +0200 Subject: [PATCH] WIP polylines.. --- CMakeLists.txt | 5 +- .../landmark/landmark_match_polyline_2D.h | 2 + include/base/processor/polyline_2D_utils.h | 2 +- ...> processor_tracker_feature_polyline_2D.h} | 22 +- src/landmark/landmark_match_polyline_2D.cpp | 66 + src/processor/polyline_2D_utils.cpp | 37 +- ...processor_tracker_feature_polyline_2D.cpp} | 298 +++-- test/gtest_polyline_2D.cpp | 161 ++- ..._processor_tracker_feature_polyline_2D.cpp | 1183 +++++++++++++++++ 9 files changed, 1667 insertions(+), 109 deletions(-) rename include/base/processor/{processor_tracker_feature_polyline.h => processor_tracker_feature_polyline_2D.h} (92%) create mode 100644 src/landmark/landmark_match_polyline_2D.cpp rename src/processor/{processor_tracker_feature_polyline.cpp => processor_tracker_feature_polyline_2D.cpp} (76%) create mode 100644 test/gtest_processor_tracker_feature_polyline_2D.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index be9884623..0728998b8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -555,6 +555,7 @@ SET(SRCS_LANDMARK src/landmark/landmark_container.cpp src/landmark/landmark_line_2D.cpp src/landmark/landmark_polyline_2D.cpp + src/landmark/landmark_match_polyline_2D.cpp ) SET(SRCS_PROCESSOR src/processor/processor_frame_nearest_neighbor_filter.cpp @@ -619,7 +620,7 @@ IF (laser_scan_utils_FOUND) include/base/processor/processor_polyline.h include/base/processor/processor_tracker_feature_corner.h include/base/processor/processor_tracker_landmark_corner.h - include/base/processor/processor_tracker_feature_polyline.h + include/base/processor/processor_tracker_feature_polyline_2D.h include/base/processor/polyline_2D_utils.h ) SET(HDRS_SENSOR ${HDRS_SENSOR} @@ -632,7 +633,7 @@ IF (laser_scan_utils_FOUND) src/processor/processor_polyline.cpp src/processor/processor_tracker_feature_corner.cpp src/processor/processor_tracker_landmark_corner.cpp - src/processor/processor_tracker_feature_polyline.cpp + src/processor/processor_tracker_feature_polyline_2D.cpp src/processor/polyline_2D_utils.cpp ) SET(SRCS_SENSOR ${SRCS_SENSOR} diff --git a/include/base/landmark/landmark_match_polyline_2D.h b/include/base/landmark/landmark_match_polyline_2D.h index f7835b0c8..24d1eddf4 100644 --- a/include/base/landmark/landmark_match_polyline_2D.h +++ b/include/base/landmark/landmark_match_polyline_2D.h @@ -25,6 +25,8 @@ struct LandmarkMatchPolyline2D : public LandmarkMatch int feature_to_id_; int landmark_version_; Eigen::Matrix3s T_feature_landmark_; + + bool check() const; }; }//end namespace diff --git a/include/base/processor/polyline_2D_utils.h b/include/base/processor/polyline_2D_utils.h index 94d262eee..78d6b960a 100644 --- a/include/base/processor/polyline_2D_utils.h +++ b/include/base/processor/polyline_2D_utils.h @@ -40,7 +40,7 @@ Eigen::Matrix3s pose2T2D(const Eigen::Vector2s& t, const Scalar& theta); Eigen::Matrix3s pose2T2D(const Eigen::Vector3s& pose); Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T); -Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last); +Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last, const bool& first_defined = true, const bool& last_defined=true); /** \brief Computes the squared distance from a point to a segment defined by two points **/ diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline_2D.h similarity index 92% rename from include/base/processor/processor_tracker_feature_polyline.h rename to include/base/processor/processor_tracker_feature_polyline_2D.h index 94ee78021..f88366880 100644 --- a/include/base/processor/processor_tracker_feature_polyline.h +++ b/include/base/processor/processor_tracker_feature_polyline_2D.h @@ -1,12 +1,12 @@ /* - * processor_tracker_feature_polyline.h + * processor_tracker_feature_polyline_2D.h * * Created on: Mar 11, 2019 * Author: jvallve */ -#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_H_ -#define PROCESSOR_TRACKER_FEATURE_POLYLINE_H_ +#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_ +#define PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_ #include "base/processor/polyline_2D_utils.h" #include "base/processor/processor_tracker_feature.h" @@ -27,14 +27,14 @@ namespace wolf { -WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline); -WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline); +WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline2D); +WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline2D); typedef std::list<FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DList; typedef std::list<LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DList; typedef std::map<Scalar,FeatureMatchPolyline2DPtr> FeatureMatchPolyline2DScalarMap; typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScalarMap; -struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature +struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature { laserscanutils::LineFinderIterativeParams line_finder_params; Scalar match_alignment_position_sq_norm_th; @@ -46,10 +46,10 @@ struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeat std::vector<PolylineRectangularClass> polyline_classes; }; -class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature +class ProcessorTrackerFeaturePolyline2D : public ProcessorTrackerFeature { protected: - ProcessorParamsTrackerFeaturePolylinePtr params_tracker_feature_polyline_; + ProcessorParamsTrackerFeaturePolyline2DPtr params_tracker_feature_polyline_; laserscanutils::LineFinderIterative line_finder_; FeatureBaseList all_features_incoming_, all_features_last_; @@ -68,9 +68,9 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature bool extrinsics_transformation_computed_; public: - ProcessorTrackerFeaturePolyline(ProcessorParamsTrackerFeaturePolylinePtr _params); + ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params); - virtual ~ProcessorTrackerFeaturePolyline(); + virtual ~ProcessorTrackerFeaturePolyline2D(); virtual void configure(SensorBasePtr _sensor){}; @@ -204,4 +204,4 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature } /* namespace wolf */ -#endif /* PROCESSOR_TRACKER_FEATURE_POLYLINE_H_ */ +#endif /* PROCESSOR_TRACKER_FEATURE_POLYLINE_2D_H_ */ diff --git a/src/landmark/landmark_match_polyline_2D.cpp b/src/landmark/landmark_match_polyline_2D.cpp new file mode 100644 index 000000000..f6028792b --- /dev/null +++ b/src/landmark/landmark_match_polyline_2D.cpp @@ -0,0 +1,66 @@ +/* + * landmark_match_polyline_2D.cpp + * + * Created on: Apr 2, 2019 + * Author: jvallve + */ + +#include "base/landmark/landmark_match_polyline_2D.h" +#include "base/landmark/landmark_polyline_2D.h" + +namespace wolf { + +bool LandmarkMatchPolyline2D::check() const +{ + LandmarkPolyline2DPtr pl_lmk = std::dynamic_pointer_cast<LandmarkPolyline2D>(this->landmark_ptr_); + if (pl_lmk == nullptr) + { + WOLF_ERROR("LandmarkMatchPolyline2D landmark is not polyline 2D"); + return false; + } + if (!pl_lmk->isClosed()) + { + if (landmark_from_id_ > landmark_to_id_) + { + WOLF_ERROR("LandmarkMatchPolyline2D lmk_from > lmk_to with a not closed lmk"); + return false; + } + if (landmark_from_id_ < pl_lmk->getFirstId()) + { + WOLF_ERROR("LandmarkMatchPolyline2D lmk_from < lmk_first with a not closed lmk"); + return false; + } + if (landmark_to_id_ > pl_lmk->getLastId()) + { + WOLF_ERROR("LandmarkMatchPolyline2D lmk_to < lmk_last with a not closed lmk"); + return false; + } + } + int lmk_id = landmark_from_id_; + int ftr_id = feature_from_id_; + while (1) + { + if (lmk_id == landmark_to_id_) + { + if (ftr_id != feature_to_id_) + { + WOLF_ERROR("LandmarkMatchPolyline2D landmark_to_id_ does not correspond to feature_to_id_"); + return false; + } + break; + } + else if (lmk_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) + { + WOLF_ERROR("LandmarkMatchPolyline2D lmk_id == lmk_last_id but lmk_id != landmark_to_id_ in a not closed lmk"); + return false; + } + else + { + lmk_id = pl_lmk->getNextValidId(lmk_id); + ftr_id++; + } + } + return true; +} + +} // namespace wolf diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp index 25a84f85f..fc0e62dda 100644 --- a/src/processor/polyline_2D_utils.cpp +++ b/src/processor/polyline_2D_utils.cpp @@ -31,31 +31,46 @@ Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T) return pose; } -Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen::MatrixXs& _points_B) +Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen::MatrixXs& _points_B, const bool& first_defined, const bool& last_defined) { assert(_points_A.cols() == _points_B.cols()); assert(_points_A.rows() >= 2); assert(_points_B.rows() >= 2); - assert(_points_A.cols() >= 2); + + int N_defined_points = _points_A.cols() - (first_defined ? 0 : 1) - (last_defined ? 0 : 1); + bool all_defined = first_defined && last_defined; Eigen::Vector3s t_A_B; + assert(N_defined_points > 0 && _points_A.cols() > 1); // Rotation - Eigen::VectorXs angles_A_B(_points_A.cols()); - Eigen::MatrixXs points_A_mean = _points_A.colwise()-_points_A.rowwise().mean(); - Eigen::MatrixXs points_B_mean = _points_B.colwise()-_points_B.rowwise().mean(); + Eigen::VectorXs angles_A_B(Eigen::VectorXs::Zero(_points_A.cols())); + Eigen::Vector2s points_A_mean((all_defined ? _points_A.row(0).mean() : _points_A.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()), + (all_defined ? _points_A.row(1).mean() : _points_A.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean())); + Eigen::Vector2s points_B_mean((all_defined ? _points_B.row(0).mean() : _points_B.row(0).segment(first_defined ? 0 : 1, N_defined_points).mean()), + (all_defined ? _points_B.row(1).mean() : _points_B.row(1).segment(first_defined ? 0 : 1, N_defined_points).mean())); + Eigen::MatrixXs points_A_centered = _points_A.topRows<2>().colwise()-points_A_mean; + Eigen::MatrixXs points_B_centered = _points_B.topRows<2>().colwise()-points_B_mean; - for (auto i = 0; i<_points_A.cols(); i++) - angles_A_B(i) = pi2pi(atan2(points_A_mean(1,i),points_A_mean(0,i)) - atan2(points_B_mean(1,i),points_B_mean(0,i))); + for (auto i = 0; i < _points_A.cols(); i++ ) + angles_A_B(i) = pi2pi(atan2(points_A_centered(1,i),points_A_centered(0,i)) - atan2(points_B_centered(1,i),points_B_centered(0,i))); // fix 2nd&3rd quadrant angles bad resulting mean if (angles_A_B.maxCoeff() > M_PI / 2 && angles_A_B.minCoeff() < -M_PI / 2) - angles_A_B = (angles_A_B.array() > 0).select(angles_A_B, angles_A_B.array()+2*M_PI); - - t_A_B(2) = angles_A_B.mean(); + angles_A_B = (angles_A_B.array() >= 0).select(angles_A_B, angles_A_B.array()+2*M_PI); + + // if only one defined point, mean of not defined points angles (mean is pivot) + if (N_defined_points == 1) + t_A_B(2) = ((first_defined ? 0 : angles_A_B(0)) + (last_defined ? 0 : angles_A_B(_points_A.cols()-1))) / ((first_defined ? 0 : 1) + (last_defined ? 0 : 1)); + // use all points if all defined + else if (all_defined) + t_A_B(2) = angles_A_B.mean(); + // otherwise compute using defined points + else + t_A_B(2) = angles_A_B.segment(first_defined ? 0 : 1, N_defined_points).mean(); // Translation - t_A_B.head<2>() = _points_A.topRows<2>().rowwise().mean() - Eigen::Rotation2D<Scalar>(t_A_B(2))*_points_B.topRows<2>().rowwise().mean(); + t_A_B.head<2>() = points_A_mean - Eigen::Rotation2D<Scalar>(t_A_B(2))*points_B_mean; return t_A_B; } diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp similarity index 76% rename from src/processor/processor_tracker_feature_polyline.cpp rename to src/processor/processor_tracker_feature_polyline_2D.cpp index b230252eb..b90877de9 100644 --- a/src/processor/processor_tracker_feature_polyline.cpp +++ b/src/processor/processor_tracker_feature_polyline_2D.cpp @@ -1,16 +1,16 @@ /* - * processor_tracker_feature_polyline.cpp + * processor_tracker_feature_polyline_2D.cpp * * Created on: Mar 11, 2019 * Author: jvallve */ -#include "base/processor/processor_tracker_feature_polyline.h" +#include "base/processor/processor_tracker_feature_polyline_2D.h" namespace wolf { -ProcessorTrackerFeaturePolyline::ProcessorTrackerFeaturePolyline(ProcessorParamsTrackerFeaturePolylinePtr _params) : +ProcessorTrackerFeaturePolyline2D::ProcessorTrackerFeaturePolyline2D(ProcessorParamsTrackerFeaturePolyline2DPtr _params) : ProcessorTrackerFeature("TRACKER FEATURE POLYLINE",_params), params_tracker_feature_polyline_(_params), line_finder_(_params->line_finder_params), @@ -19,11 +19,11 @@ ProcessorTrackerFeaturePolyline::ProcessorTrackerFeaturePolyline(ProcessorParams } -ProcessorTrackerFeaturePolyline::~ProcessorTrackerFeaturePolyline() +ProcessorTrackerFeaturePolyline2D::~ProcessorTrackerFeaturePolyline2D() { } -unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseList& _features_last_in, +unsigned int ProcessorTrackerFeaturePolyline2D::trackFeatures(const FeatureBaseList& _features_last_in, FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) { @@ -93,8 +93,18 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis else if (pl_lmk->isRemoving()) continue; + // try to concatenate landmark match + auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming, + best_ftr_match, + landmark_match_map_[pl_last]); // Add the incoming match to landmark_match_map - landmark_match_map_[pl_incoming] = concatenateFeatureLandmarkMatches(pl_incoming, best_ftr_match, landmark_match_map_[pl_last]); + if (pl_lmk_match_incoming != nullptr) + landmark_match_map_[pl_incoming] = pl_lmk_match_incoming; + else + { + WOLF_DEBUG("PTFP ", getName(), "::trackFeatures: incoming track lost common points with landmark"); + continue; + } } // TRACK valid @@ -122,9 +132,9 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis return _feature_correspondences.size(); } -bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _origin_feature, - const FeatureBasePtr _last_feature, - FeatureBasePtr _incoming_feature) +bool ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(const FeatureBasePtr _origin_feature, + const FeatureBasePtr _last_feature, + FeatureBasePtr _incoming_feature) { //WOLF_DEBUG("PTFP ", getName(), "::correctFeatureDrift: "); // If incoming observed new landmark points & landmark has points @@ -138,7 +148,7 @@ bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _ return true; } -bool ProcessorTrackerFeaturePolyline::voteForKeyFrame() +bool ProcessorTrackerFeaturePolyline2D::voteForKeyFrame() { //WOLF_DEBUG("PTFP ", getName(), "::voteForKeyFrame: "); // TODO @@ -151,7 +161,7 @@ bool ProcessorTrackerFeaturePolyline::voteForKeyFrame() return false; } -unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _max_features) +unsigned int ProcessorTrackerFeaturePolyline2D::processNew(const unsigned int& _max_features) { WOLF_DEBUG("PTFP ", getName(), "::processNew: "); unsigned int n = ProcessorTrackerFeature::processNew(_max_features); @@ -224,23 +234,35 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma } // store in landmark_match_map_ the incoming features - for (auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n); - ftr_it != incoming_ptr_->getFeatureList().end(); - ftr_it++) + auto ftr_it = std::prev(incoming_ptr_->getFeatureList().end(),n); + while (ftr_it != incoming_ptr_->getFeatureList().end()) { auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); assert(matches_last_from_incoming_.find(pl_incoming) != matches_last_from_incoming_.end() && "last-incoming match not found"); auto pl_ftr_match = std::static_pointer_cast<FeatureMatchPolyline2D>(matches_last_from_incoming_[pl_incoming]); assert(landmark_match_map_.find(pl_ftr_match->feature_ptr_) != landmark_match_map_.end() && "last-lmk match not found"); - landmark_match_map_[pl_incoming] = concatenateFeatureLandmarkMatches(pl_incoming, - pl_ftr_match, - landmark_match_map_[pl_ftr_match->feature_ptr_]); + auto pl_lmk_match_incoming = concatenateFeatureLandmarkMatches(pl_incoming, + pl_ftr_match, + landmark_match_map_[pl_ftr_match->feature_ptr_]); + if (pl_lmk_match_incoming != nullptr) + { + landmark_match_map_[pl_incoming] = pl_lmk_match_incoming; + ftr_it++; + } + else + { + WOLF_DEBUG("PTFP ", getName(), "::processNew: incoming track lost common points with landmark"); + matches_last_from_incoming_.erase(pl_incoming); + // move feature from known to all features + all_features_incoming_.push_back(pl_incoming); + ftr_it = incoming_ptr_->getFeatureList().erase(ftr_it); + } } return n; } -unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned int& _max_new_features, - FeatureBaseList& _features_last_out) +unsigned int ProcessorTrackerFeaturePolyline2D::detectNewFeatures(const unsigned int& _max_new_features, + FeatureBaseList& _features_last_out) { WOLF_DEBUG("PTFP ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)"); int prev_size = _features_last_out.size(); @@ -265,7 +287,7 @@ unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned i return 0; } -void ProcessorTrackerFeaturePolyline::establishConstraints() +void ProcessorTrackerFeaturePolyline2D::establishConstraints() { WOLF_DEBUG("PTFP ", getName(), "::establishConstraints: "); unsigned int N_constraints = 0; @@ -322,16 +344,19 @@ void ProcessorTrackerFeaturePolyline::establishConstraints() // ESTABLISH CONSTRAINTS WOLF_DEBUG("\tEstablishing constraints.."); + // checks assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly"); - assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + lmk_match->landmark_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly"); - //std::cout << "Establishing constraint between" << std::endl; - //std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; - //std::cout << "\tlandmark " << pl_lmk->id() << std::endl; - //std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl; - //std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl; - //std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl; - //std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl; + assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); + assert(lmk_match->check() && "Landmark didn't grow properly"); + + std::cout << "Establishing constraint between" << std::endl; + std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is linked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl; + std::cout << "\tlandmark " << pl_lmk->id() << std::endl; + std::cout << "\tmatch from feature point " << lmk_match->feature_from_id_ << std::endl; + std::cout << "\tmatch to feature point " << lmk_match->feature_to_id_ << std::endl; + std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl; + std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl; // Constraints for all feature points int lmk_point_id = lmk_match->landmark_from_id_; @@ -389,11 +414,11 @@ void ProcessorTrackerFeaturePolyline::establishConstraints() std::cout << N_constraints << " constraints established" << std::endl; } -void ProcessorTrackerFeaturePolyline::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature, - LandmarkPolyline2DPtr _polyline_landmark, - const int& _ftr_point_id, - const int& _lmk_point_id, - const int& _lmk_prev_point_id) +void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature, + LandmarkPolyline2DPtr _polyline_landmark, + const int& _ftr_point_id, + const int& _lmk_point_id, + const int& _lmk_prev_point_id) { assert(_polyline_landmark->isValidId(_lmk_point_id) && _polyline_landmark->isValidId(_lmk_prev_point_id)); @@ -405,10 +430,10 @@ void ProcessorTrackerFeaturePolyline::emplaceConstraintPointToLine(FeaturePolyli _polyline_landmark->addConstrainedBy(new_ctr); } -void ProcessorTrackerFeaturePolyline::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature, - LandmarkPolyline2DPtr _polyline_landmark, - const int& _ftr_point_id, - const int& _lmk_point_id) +void ProcessorTrackerFeaturePolyline2D::emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature, + LandmarkPolyline2DPtr _polyline_landmark, + const int& _ftr_point_id, + const int& _lmk_point_id) { // CREATE CONSTRAINT -------------------- ConstraintBasePtr new_ctr = std::make_shared<ConstraintPoint2D>(_polyline_feature, _polyline_landmark, shared_from_this(), _ftr_point_id, _lmk_point_id); @@ -418,7 +443,7 @@ void ProcessorTrackerFeaturePolyline::emplaceConstraintPoint(FeaturePolyline2DPt _polyline_landmark->addConstrainedBy(new_ctr); } -LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _feature_ptr) +LandmarkBasePtr ProcessorTrackerFeaturePolyline2D::createLandmark(FeatureBasePtr _feature_ptr) { WOLF_DEBUG("PTFP ", getName(), "::createLandmark: "); //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl; @@ -445,12 +470,12 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _ //std::cout << "done" << std::endl; } -bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr) +bool ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr) { WOLF_DEBUG("PTFP ", getName(), "::modifyLandmarkAndMatch: "); auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_); - bool print = false; + bool print = true; bool modified = false; if (print) @@ -469,8 +494,9 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli std::cout << "\tmatch from landmark point " << lmk_match->landmark_from_id_ << std::endl; std::cout << "\tmatch to landmark point " << lmk_match->landmark_to_id_ << std::endl; } - Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() + - t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose(); + //Eigen::MatrixXs points_global = R_world_sensor_last_ * pl_ftr->getPoints().topRows<2>() + + // t_world_sensor_last_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose(); + Eigen::MatrixXs points_global = lmk_match->T_feature_landmark_.inverse() * pl_ftr->getPoints(); // MODIFY LANDMARK // -----------------GROW Front----------------- @@ -495,10 +521,18 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli lmk_match->feature_from_id_ = 0; lmk_match->landmark_version_ = pl_lmk->getVersion(); modified = true; - //std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl; - //std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl; - //std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl; - //std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl; + if (print) + { + std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl; + std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl; + std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl; + std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl; + } + + // checks + assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); + assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); + assert(lmk_match->check() && "Landmark didn't grow properly"); } // -----------------CHANGE Front----------------- // Change first not defined point if landmark first not defined matched with first feature point that: @@ -524,8 +558,12 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli modified = true; lmk_match->landmark_version_ = pl_lmk->getVersion(); } + + // checks + assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); + assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); + assert(lmk_match->check() && "Landmark didn't grow properly"); } - assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); // -----------------GROW Back----------------- // Add new back points (if any feature point not matched) if (lmk_match->feature_to_id_ < pl_ftr->getNPoints()-1) @@ -548,10 +586,18 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli lmk_match->feature_to_id_ = pl_ftr->getNPoints()-1; lmk_match->landmark_version_ = pl_lmk->getVersion(); modified = true; - //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl; - //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl; - //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl; - //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl; + if (print) + { + std::cout << "\tfeat from " << lmk_match->feature_from_id_ << std::endl; + std::cout << "\tfeat to " << lmk_match->feature_to_id_ << std::endl; + std::cout << "\tland from " << lmk_match->landmark_from_id_ << std::endl; + std::cout << "\tland to " << lmk_match->landmark_to_id_ << std::endl; + } + // checks + assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); + assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly"); + assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); + assert(lmk_match->check() && "Landmark didn't grow properly"); } // -----------------CHANGE Back----------------- // Change last not defined point if landmark last not defined matched with last feature point that: @@ -577,6 +623,12 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli lmk_match->landmark_version_ = pl_lmk->getVersion(); modified = true; } + + // checks + assert(lmk_match->feature_from_id_ == 0 && "Landmark didn't grow properly"); + assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly"); + assert(pl_lmk->getNPoints() >= pl_ftr->getNPoints() && "Landmark didn't grow properly"); + assert(lmk_match->check() && "Landmark didn't grow properly"); } assert(lmk_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly"); @@ -596,7 +648,7 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli return modified; } -void ProcessorTrackerFeaturePolyline::advanceDerived() +void ProcessorTrackerFeaturePolyline2D::advanceDerived() { WOLF_DEBUG("PTFP ", getName(), "::advanceDerived: "); // remove last features from landmark_match_map_ @@ -616,7 +668,7 @@ void ProcessorTrackerFeaturePolyline::advanceDerived() ProcessorTrackerFeature::advanceDerived(); } -void ProcessorTrackerFeaturePolyline::resetDerived() +void ProcessorTrackerFeaturePolyline2D::resetDerived() { WOLF_DEBUG("PTFP ", getName(), "::resetDerived: "); // remove last features from landmark_match_map_ @@ -637,7 +689,7 @@ void ProcessorTrackerFeaturePolyline::resetDerived() ProcessorTrackerFeature::resetDerived(); } -void ProcessorTrackerFeaturePolyline::preProcess() +void ProcessorTrackerFeaturePolyline2D::preProcess() { WOLF_DEBUG("PTFP ", getName(), "::extractPolylines: "); // Extract all polylines from incoming_ @@ -664,7 +716,7 @@ void ProcessorTrackerFeaturePolyline::preProcess() WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); } -void ProcessorTrackerFeaturePolyline::postProcess() +void ProcessorTrackerFeaturePolyline2D::postProcess() { WOLF_DEBUG("PTFP ", getName(), "::postProcess: "); // Try to close & classify modified landmarks @@ -710,10 +762,10 @@ void ProcessorTrackerFeaturePolyline::postProcess() WOLF_DEBUG("PTF ", getName(), ": ", last_ptr_->getFeatureList().size(), " in last_ptr_)"); } -void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, - const FeaturePolyline2DPtr& _ftr_L, - const FeaturePolyline2DPtr& _ftr_I, - const Eigen::Matrix3s& _T_last_incoming_prior) const +void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, + const FeaturePolyline2DPtr& _ftr_L, + const FeaturePolyline2DPtr& _ftr_I, + const Eigen::Matrix3s& _T_last_incoming_prior) const { //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithFeature: "); @@ -740,7 +792,19 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2D ftr_match->feature_last_from_id_ = best_match->from_B_id_; ftr_match->feature_last_to_id_ = best_match->to_B_id_; // incoming last transformation - //ftr_match->T_incoming_last_ = match->T_A_B_; unavailable data + bool from_defined = (ftr_match->feature_incoming_from_id_ != 0 || _ftr_I->isFirstDefined()) && + (ftr_match->feature_last_from_id_ != 0 || _ftr_L->isFirstDefined()); + bool to_defined = (ftr_match->feature_incoming_to_id_ != _ftr_I->getNPoints()-1 || _ftr_I->isLastDefined()) && + (ftr_match->feature_last_to_id_ != _ftr_L->getNPoints()-1 || _ftr_L->isLastDefined()); + int N_points = ftr_match->feature_incoming_to_id_ - ftr_match->feature_incoming_from_id_; + int N_def_points = N_points - (from_defined ? 0:1) - (to_defined ? 0:1); + if (N_def_points > 0 && N_points > 2) + ftr_match->T_incoming_last_ = pose2T2D(computeRigidTrans(_ftr_I->getPoints().middleCols(ftr_match->feature_incoming_from_id_,N_points), + _ftr_L->getPoints().middleCols(ftr_match->feature_last_from_id_,N_points), + from_defined, + to_defined)); + else + ftr_match->T_incoming_last_ = _T_last_incoming_prior; // score ftr_match->normalized_score_ = best_match->normalized_score_; // feature correspondence @@ -793,10 +857,10 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2D // } } -void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, - const LandmarkPolyline2DPtr& _lmk_ptr, - const FeaturePolyline2DPtr& _feat_ptr, - const Eigen::Matrix3s& _T_feature_landmark_prior) const +void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, + const LandmarkPolyline2DPtr& _lmk_ptr, + const FeaturePolyline2DPtr& _feat_ptr, + const Eigen::Matrix3s& _T_feature_landmark_prior) const { //WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: "); @@ -827,8 +891,22 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline lmk_match->feature_from_id_ = best_match->from_B_id_; lmk_match->feature_to_id_ = best_match->to_B_id_; - // incoming last transformation + // feature landmark transformation lmk_match->T_feature_landmark_ = best_match->T_A_B_.inverse().eval(); + + bool from_defined = (lmk_match->landmark_from_id_ != _lmk_ptr->getFirstId() || _lmk_ptr->isFirstDefined()) && + (lmk_match->feature_from_id_ != 0 || _feat_ptr->isFirstDefined()); + bool to_defined = (lmk_match->landmark_to_id_ != _lmk_ptr->getLastId() || _lmk_ptr->isLastDefined()) && + (lmk_match->feature_to_id_ != _feat_ptr->getNPoints()-1 || _feat_ptr->isLastDefined()); + int N_points = lmk_match->feature_to_id_ - lmk_match->feature_from_id_; + int N_def_points = N_points - (from_defined ? 0:1) - (to_defined ? 0:1); + if (N_def_points > 0 && N_points > 2) + lmk_match->T_feature_landmark_ = pose2T2D(computeRigidTrans(_feat_ptr->getPoints().middleCols(lmk_match->feature_from_id_,N_points), + _lmk_ptr->computePointsGlobal().middleCols(lmk_match->landmark_from_id_,N_points), + from_defined, + to_defined)); + else + lmk_match->T_feature_landmark_ = _T_feature_landmark_prior; // score lmk_match->normalized_score_ = best_match->normalized_score_; @@ -838,6 +916,7 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline // store in list lmk_matches[lmk_match->normalized_score_]= lmk_match; WOLF_DEBUG("match stored!"); + assert(lmk_match->check() && "tryMatchWithLandmark: wrong match"); } // // compute best rigid transformation matching @@ -884,8 +963,11 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline // } } -bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const +bool ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, + LandmarkPolyline2DPtr& _lmk_ptr, + FeaturePolyline2DPtr& _feat_ptr) const { + assert(_lmk_match->check() && "updateMatchWithLandmark: input _lmk_match wrong"); WOLF_DEBUG("PTFP ", getName(), "::updateMatchWithLandmark: "); assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id()); @@ -905,34 +987,88 @@ bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyl } // valid match: update landmark match of last + assert(_lmk_match->check() && "updateMatchWithLandmark: wrongly updated"); _lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match return true; } -LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming, - FeatureMatchPolyline2DPtr ftr_match, - LandmarkMatchPolyline2DPtr lmk_match_last) const +LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming, + FeatureMatchPolyline2DPtr ftr_match, + LandmarkMatchPolyline2DPtr lmk_match_last) const { - // Add the match to landmark_match_map + assert(lmk_match_last->check() && "input lmk_match_last wrong"); + WOLF_DEBUG("PTFP ", getName(), "::concatenateFeatureLandmarkMatches: "); + std::cout << "ftr_match:"; + std::cout << "\n\tincoming_from: " << ftr_match->feature_incoming_from_id_; + std::cout << "\n\tincoming_to: " << ftr_match->feature_incoming_to_id_; + std::cout << "\n\tlast_from: " << ftr_match->feature_last_from_id_; + std::cout << "\n\tlast_to: " << ftr_match->feature_last_to_id_; + std::cout << "\nlmk_match_last:"; + std::cout << "\n\tlast_from: " << lmk_match_last->feature_from_id_; + std::cout << "\n\tlast_to: " << lmk_match_last->feature_to_id_; + std::cout << "\n\tlmk_from: " << lmk_match_last->landmark_from_id_; + std::cout << "\n\tlmk_to: " << lmk_match_last->landmark_to_id_; + auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>(); + auto lmk_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_); + // feature correspondence from&to lmk_match_incoming->feature_from_id_ = ftr_match->feature_incoming_from_id_; lmk_match_incoming->feature_to_id_ = ftr_match->feature_incoming_to_id_; + + // landmark correspondence from&to // Not adding new points nor checks (done in correctFeatureDrift) - lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_ + ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_; - lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_ + ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_; - lmk_match_incoming->landmark_ptr_ = lmk_match_last->landmark_ptr_; - lmk_match_incoming->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_)->getVersion(); + int from_offset = ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_; + int to_offset = ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_; + std::cout << "\nfrom_offset: " << from_offset; + std::cout << "\nto_offset: " << from_offset; + lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_; + lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_; + while (from_offset > 0) + { + if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed()) + return nullptr; + lmk_match_incoming->landmark_from_id_ = lmk_ptr->getNextValidId(lmk_match_incoming->landmark_from_id_); + from_offset--; + } + while (from_offset < 0) + { + lmk_match_incoming->feature_from_id_++; + from_offset++; + } + while (to_offset < 0) + { + if (lmk_match_incoming->landmark_from_id_ == lmk_match_incoming->landmark_to_id_ && !lmk_ptr->isClosed()) + return nullptr; + lmk_match_incoming->landmark_to_id_ = lmk_ptr->getPrevValidId(lmk_match_incoming->landmark_to_id_); + to_offset++; + } + while (to_offset > 0) + { + lmk_match_incoming->feature_to_id_--; + to_offset--; + } + std::cout << "\nlmk_match_incoming:"; + std::cout << "\n\tincoming_from: " << lmk_match_incoming->feature_from_id_; + std::cout << "\n\tincoming_to: " << lmk_match_incoming->feature_to_id_; + std::cout << "\n\tlmk_from: " << lmk_match_incoming->landmark_from_id_; + std::cout << "\n\tlmk_to: " << lmk_match_incoming->landmark_to_id_ << std::endl; + + // other match attributes + lmk_match_incoming->landmark_ptr_ = lmk_ptr; + lmk_match_incoming->landmark_version_ = lmk_ptr->getVersion(); lmk_match_incoming->normalized_score_ = ftr_match->normalized_score_; lmk_match_incoming->T_feature_landmark_ = ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_; + assert(lmk_match_incoming->check() && "lmk_match_incoming wrongly concatenated"); + return lmk_match_incoming; } // MATH /////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -void ProcessorTrackerFeaturePolyline::computeTransformations() +void ProcessorTrackerFeaturePolyline2D::computeTransformations() { //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: "); Eigen::Vector3s vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp()); @@ -996,12 +1132,12 @@ void ProcessorTrackerFeaturePolyline::computeTransformations() } -ProcessorBasePtr ProcessorTrackerFeaturePolyline::create(const std::string& _unique_name, - const ProcessorParamsBasePtr _params, - const SensorBasePtr _sensor_ptr) +ProcessorBasePtr ProcessorTrackerFeaturePolyline2D::create(const std::string& _unique_name, + const ProcessorParamsBasePtr _params, + const SensorBasePtr _sensor_ptr) { - auto params = std::static_pointer_cast<ProcessorParamsTrackerFeaturePolyline>(_params); - auto prc_ptr = std::make_shared<ProcessorTrackerFeaturePolyline>(params); + auto params = std::static_pointer_cast<ProcessorParamsTrackerFeaturePolyline2D>(_params); + auto prc_ptr = std::make_shared<ProcessorTrackerFeaturePolyline2D>(params); prc_ptr->setName(_unique_name); prc_ptr->configure(_sensor_ptr); return prc_ptr; @@ -1012,5 +1148,5 @@ ProcessorBasePtr ProcessorTrackerFeaturePolyline::create(const std::string& _uni // Register in the SensorFactory #include "base/processor/processor_factory.h" namespace wolf { -WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline) +WOLF_REGISTER_PROCESSOR("TRACKER FEATURE POLYLINE", ProcessorTrackerFeaturePolyline2D) } /* namespace wolf */ diff --git a/test/gtest_polyline_2D.cpp b/test/gtest_polyline_2D.cpp index d8c50e392..05d2f706f 100644 --- a/test/gtest_polyline_2D.cpp +++ b/test/gtest_polyline_2D.cpp @@ -12,7 +12,7 @@ TEST(Polyline2DTest, Transformations) ASSERT_MATRIX_APPROX(Vector3s::Zero(), T2pose2D(Matrix3s::Identity()), 1e-12); } -TEST(Polyline2DTest, RigidTransformation) +TEST(Polyline2DTest, RigidTransformation8points) { // Random points around random position MatrixXs points_last = MatrixXs::Random(3,8); @@ -21,7 +21,7 @@ TEST(Polyline2DTest, RigidTransformation) points_last = T_random * points_last; // For different orientation changes between last and incomming - for (Scalar theta_last_incoming = -M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI) + for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI) { // movement from last to incoming (random translation) Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix(); @@ -39,12 +39,167 @@ TEST(Polyline2DTest, RigidTransformation) Vector3s v = computeRigidTrans(points_incoming, points_last); ASSERT_MATRIX_APPROX(v.head(2), t_incoming_last, 1e-12); - ASSERT_DOUBLE_EQ(v(2), theta_incoming_last); + ASSERT_NEAR(v(2), theta_incoming_last, 1e-12); } // PRINTF("All good at TestTest::DummyTestExample !\n"); } +TEST(Polyline2DTest, RigidTransformation2points2defined) +{ + // Random points around random position + MatrixXs points_last = MatrixXs::Random(3,2); + points_last.bottomRows(1) = MatrixXs::Ones(1,2); + MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001); + points_last = T_random * points_last; + + // For different orientation changes between last and incomming + for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI) + { + // movement from last to incoming (random translation) + Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix(); + Vector2s t_last_incoming(Vector2s::Random()); + + // inverse transformation + Scalar theta_incoming_last = -theta_last_incoming; + Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming; + Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last); + + // rotate points + MatrixXs points_incoming = T_incoming_last * points_last; + + // compute rigit transformation + Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last); + Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed); + + ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.rightCols(2), points_incoming.rightCols(2), 1e-12); + ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12); + ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12); + } + +// PRINTF("All good at TestTest::DummyTestExample !\n"); +} + +TEST(Polyline2DTest, RigidTransformation3points2defined) +{ + // Random points around random position + MatrixXs points_last = MatrixXs::Random(3,3); + points_last.bottomRows(1) = MatrixXs::Ones(1,3); + MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001); + points_last = T_random * points_last; + + // For different orientation changes between last and incomming + for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI) + { + // movement from last to incoming (random translation) + Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix(); + Vector2s t_last_incoming(Vector2s::Random()); + + // inverse transformation + Scalar theta_incoming_last = -theta_last_incoming; + Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming; + Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last); + + // rotate points + MatrixXs points_incoming = T_incoming_last * points_last; + + // change not defined point (first) + points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(0) - points_incoming.col(1)); + points_incoming(2,0) = 1; + + // compute rigit transformation + Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, true); + Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed); + + ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.rightCols(2), points_incoming.rightCols(2), 1e-12); + ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12); + ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12); + } + +// PRINTF("All good at TestTest::DummyTestExample !\n"); +} + +TEST(Polyline2DTest, RigidTransformation3points1defined) +{ + // Random points around random position + MatrixXs points_last = MatrixXs::Random(3,3); + points_last.bottomRows(1) = MatrixXs::Ones(1,3); + MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001); + points_last = T_random * points_last; + + // For different orientation changes between last and incomming + for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI) + { + // movement from last to incoming (random translation) + Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix(); + Vector2s t_last_incoming(Vector2s::Random()); + + // inverse transformation + Scalar theta_incoming_last = -theta_last_incoming; + Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming; + Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last); + + // rotate points + MatrixXs points_incoming = T_incoming_last * points_last; + + // change 2 not defined points + points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(1) - points_incoming.col(0)); + points_incoming(2,0) = 1; + points_incoming.col(2) = points_incoming.col(2) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(2) - points_incoming.col(1)); + points_incoming(2,2) = 1; + + // compute rigit transformation + Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, false); + Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed); + + ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.col(1), points_incoming.col(1), 1e-12); + ASSERT_MATRIX_APPROX(v_incoming_last_computed.head(2), t_incoming_last, 1e-12); + ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12); + } + +// PRINTF("All good at TestTest::DummyTestExample !\n"); +} + +TEST(Polyline2DTest, RigidTransformation2points1defined) +{ + // Random points around random position + MatrixXs points_last = MatrixXs::Random(3,2); + points_last.bottomRows(1) = MatrixXs::Ones(1,2); + MatrixXs T_random = pose2T2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001); + points_last = T_random * points_last; + + // For different orientation changes between last and incomming + for (Scalar theta_last_incoming = -0.9*M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI) + { + // movement from last to incoming (random translation) + Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix(); + Vector2s t_last_incoming(Vector2s::Random()); + + // inverse transformation + Scalar theta_incoming_last = -theta_last_incoming; + Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming; + Matrix3s T_incoming_last = pose2T2D(t_incoming_last, theta_incoming_last); + + // rotate points + MatrixXs points_incoming = T_incoming_last * points_last; + + // change not defined point + points_incoming.col(0) = points_incoming.col(0) + (rand() % 1000 - 500)*0.001 * (points_incoming.col(0) - points_incoming.col(1)); + points_incoming(2,0) = 1; + + // compute rigit transformation + Vector3s v_incoming_last_computed = computeRigidTrans(points_incoming, points_last, false, true); + Matrix3s T_incoming_last_computed = pose2T2D(v_incoming_last_computed); + + ASSERT_MATRIX_APPROX(T_incoming_last_computed*points_last.col(1), points_incoming.col(1), 1e-12); + ASSERT_MATRIX_APPROX(v_incoming_last_computed .head(2), t_incoming_last, 1e-12); + ASSERT_NEAR(v_incoming_last_computed(2), theta_incoming_last, 1e-12); + } + +// PRINTF("All good at TestTest::DummyTestExample !\n"); +} + + int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); diff --git a/test/gtest_processor_tracker_feature_polyline_2D.cpp b/test/gtest_processor_tracker_feature_polyline_2D.cpp new file mode 100644 index 000000000..2c3580aed --- /dev/null +++ b/test/gtest_processor_tracker_feature_polyline_2D.cpp @@ -0,0 +1,1183 @@ +/** + * \file gtest_processor_tracker_feature_polyline_2D.cpp + * + * Created on: Apr 3, 2019 + * \author: jvallve + */ + +#include "base/processor/processor_tracker_feature_polyline_2D.h" +#include "base/wolf.h" + +#include "utils_gtest.h" +#include "base/logging.h" + +#include "base/ceres_wrapper/ceres_manager.h" + +#include <cmath> +#include <iostream> + +using namespace Eigen; + +WOLF_PTR_TYPEDEFS(ProcessorPolylinePublicMethods); +class ProcessorPolylinePublicMethods : public ProcessorTrackerFeaturePolyline2D +{ + public: + ProcessorPolylinePublicMethods(ProcessorParamsTrackerFeaturePolyline2DPtr _params) : + ProcessorTrackerFeaturePolyline2D(_params) + { + + } + + unsigned int trackFeatures(const FeatureBaseList& _features_last_in, + FeatureBaseList& _features_incoming_out, + FeatureMatchMap& _feature_correspondences) + { + return ProcessorTrackerFeaturePolyline2D::trackFeatures(_features_last_in,_features_incoming_out,_feature_correspondences); + } + + bool correctFeatureDrift(const FeatureBasePtr _origin_feature, + const FeatureBasePtr _last_feature, + FeatureBasePtr _incoming_feature) + { + return ProcessorTrackerFeaturePolyline2D::correctFeatureDrift(_origin_feature, + _last_feature, + _incoming_feature); + } + + bool voteForKeyFrame() + { + return ProcessorTrackerFeaturePolyline2D::voteForKeyFrame(); + } + + unsigned int processNew(const unsigned int& _max_features) + { + return ProcessorTrackerFeaturePolyline2D::processNew(_max_features); + } + + unsigned int detectNewFeatures(const unsigned int& _max_new_features, + FeatureBaseList& _features_last_out) + { + return ProcessorTrackerFeaturePolyline2D::detectNewFeatures(_max_new_features, _features_last_out); + } + + void establishConstraints() + { + ProcessorTrackerFeaturePolyline2D::establishConstraints(); + } + + void emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature, + LandmarkPolyline2DPtr _polyline_landmark, + const int& _ftr_point_id, + const int& _lmk_point_id, + const int& _lmk_prev_point_id) + { + ProcessorTrackerFeaturePolyline2D::emplaceConstraintPointToLine(_polyline_feature, + _polyline_landmark, + _ftr_point_id, + _lmk_point_id, + _lmk_prev_point_id); + } + + void emplaceConstraintPoint(FeaturePolyline2DPtr _polyline_feature, + LandmarkPolyline2DPtr _polyline_landmark, + const int& _ftr_point_id, + const int& _lmk_point_id) + { + ProcessorTrackerFeaturePolyline2D::emplaceConstraintPoint(_polyline_feature, + _polyline_landmark, + _ftr_point_id, + _lmk_point_id); + } + + LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr) + { + return ProcessorTrackerFeaturePolyline2D::createLandmark(_feature_ptr); + } + + bool modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr) + { + return ProcessorTrackerFeaturePolyline2D::modifyLandmarkAndMatch( lmk_match, pl_ftr); + } + + void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, + const FeaturePolyline2DPtr& _ftr_last, + const FeaturePolyline2DPtr& _ftr_incoming, + const Eigen::Matrix3s& _T_last_incoming_prior) const + { + ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature( ftr_matches, + _ftr_last, + _ftr_incoming, + _T_last_incoming_prior); + } + + void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, + const LandmarkPolyline2DPtr& _lmk_ptr, + const FeaturePolyline2DPtr& _feat_ptr, + const Eigen::Matrix3s& _T_feature_landmark_prior) const + { + ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(lmk_matches, + _lmk_ptr, + _feat_ptr, + _T_feature_landmark_prior); + } + + bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) const + { + return ProcessorTrackerFeaturePolyline2D::updateMatchWithLandmark(_lmk_match, _lmk_ptr, _feat_ptr); + } + + void computeTransformations() + { + ProcessorTrackerFeaturePolyline2D::computeTransformations(); + } + + LandmarkMatchPolyline2DPtr concatenateFeatureLandmarkMatches(FeaturePolyline2DPtr pl_incoming, + FeatureMatchPolyline2DPtr ftr_match_incoming_last, + LandmarkMatchPolyline2DPtr lmk_match_last) const + { + return ProcessorTrackerFeaturePolyline2D::concatenateFeatureLandmarkMatches(pl_incoming, + ftr_match_incoming_last, + lmk_match_last); + } +}; + +class ProcessorPolyline2Dt : public testing::Test +{ + public: //These can be accessed in fixtures + ProcessorPolylinePublicMethodsPtr processor_pl; + wolf::SensorBasePtr sensor_ptr; + wolf::TimeStamp t; + wolf::Scalar mti_clock, tmp; + wolf::Scalar dt; + Vector6s data; + Matrix6s data_cov; + VectorXs x0; + std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr; + + //a new of this will be created for each new test + virtual void SetUp() + { + using namespace wolf; + using namespace Eigen; + using std::shared_ptr; + using std::make_shared; + using std::static_pointer_cast; + using namespace wolf::Constants; + + std::string wolf_root = _WOLF_ROOT_DIR; + + // Wolf problem + problem = Problem::create("POV 3D"); + Vector7s extrinsics = (Vector7s() << 0,0,0, 0,0,0,1).finished(); + sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml"); + ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"); + + // Time and data variables + data = Vector6s::Zero(); + data_cov = Matrix6s::Identity(); + + // Set the origin + x0.resize(10); + + // Create one capture to store the IMU data arriving from (sensor / callback / file / etc.) + cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero()); + } + + virtual void TearDown() + { + // code here will be called just after the test completes + // ok to through exceptions from here if need be + /* + You can do deallocation of resources in TearDown or the destructor routine. + However, if you want exception handling you must do it only in the TearDown code because throwing an exception + from the destructor results in undefined behavior. + The Google assertion macros may throw exceptions in platforms where they are enabled in future releases. + Therefore, it's a good idea to use assertion macros in the TearDown code for better maintenance. + */ + } +}; + +TEST(ProcessorIMU_constructors, ALL) +{ + using namespace wolf; + + //constructor with ProcessorIMUParamsPtr argument only + ProcessorParamsIMUPtr param_ptr = std::make_shared<ProcessorParamsIMU>(); + param_ptr->max_time_span = 2.0; + param_ptr->max_buff_length = 20000; + param_ptr->dist_traveled = 2.0; + param_ptr->angle_turned = 2.0; + + ProcessorIMUPtr prc1 = std::make_shared<ProcessorIMU>(param_ptr); + ASSERT_EQ(prc1->getMaxTimeSpan(), param_ptr->max_time_span); + ASSERT_EQ(prc1->getMaxBuffLength(), param_ptr->max_buff_length); + ASSERT_EQ(prc1->getDistTraveled(), param_ptr->dist_traveled); + ASSERT_EQ(prc1->getAngleTurned(), param_ptr->angle_turned); + + //Factory constructor without yaml + std::string wolf_root = _WOLF_ROOT_DIR; + ProblemPtr problem = Problem::create("POV 3D"); + Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished(); + SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml"); + ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", "Main IMU", ""); + ProcessorParamsIMU params_default; + ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), params_default.max_time_span); + ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), params_default.max_buff_length); + ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), params_default.dist_traveled); + ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), params_default.angle_turned); + + //Factory constructor with yaml + processor_ptr = problem->installProcessor("IMU", "Sec IMU pre-integrator", "Main IMU", wolf_root + "/src/examples/processor_imu.yaml"); + ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan(), 2.0); + ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxBuffLength(), 20000); + ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getDistTraveled(), 2.0); + ASSERT_EQ(std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getAngleTurned(), 0.2); +} + +TEST(ProcessorIMU, voteForKeyFrame) +{ + using namespace wolf; + using namespace Eigen; + using std::shared_ptr; + using std::make_shared; + using std::static_pointer_cast; + using namespace wolf::Constants; + + std::string wolf_root = _WOLF_ROOT_DIR; + + // Wolf problem + ProblemPtr problem = Problem::create("POV 3D"); + Vector7s extrinsics = (Vector7s()<<1,0,0, 0,0,0,1).finished(); + SensorBasePtr sensor_ptr = problem->installSensor("IMU", "Main IMU", extrinsics, wolf_root + "/src/examples/sensor_imu.yaml"); + ProcessorParamsIMUPtr prc_imu_params = std::make_shared<ProcessorParamsIMU>(); + prc_imu_params->max_time_span = 1; + prc_imu_params->max_buff_length = 1000000000; //make it very high so that this condition will not pass + prc_imu_params->dist_traveled = 1000000000; + prc_imu_params->angle_turned = 1000000000; + prc_imu_params->voting_active = true; + ProcessorBasePtr processor_ptr = problem->installProcessor("IMU", "IMU pre-integrator", sensor_ptr, prc_imu_params); + + //setting origin + VectorXs x0(10); + TimeStamp t(0); + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + //data variable and covariance matrix + // since we integrate only a few times, give the capture a big covariance, otherwise it will be so small that it won't pass following assertions + Vector6s data; + data << 1,0,0, 0,0,0; + Matrix6s data_cov(Matrix6s::Identity()); + data_cov(0,0) = 0.5; + + // Create the captureIMU to store the IMU data arriving from (sensor / callback / file / etc.) + std::shared_ptr<wolf::CaptureIMU> cap_imu_ptr = make_shared<CaptureIMU>(t, sensor_ptr, data, data_cov, Vector6s::Zero()); + + // Time + // we want more than one data to integrate otherwise covariance will be 0 + Scalar dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() - 0.1; + t.set(dt); + cap_imu_ptr->setTimeStamp(t); + sensor_ptr->process(cap_imu_ptr); + + dt = std::static_pointer_cast<ProcessorIMU>(processor_ptr)->getMaxTimeSpan() + 0.1; + t.set(dt); + cap_imu_ptr->setTimeStamp(t); + sensor_ptr->process(cap_imu_ptr); + + /*There should be 3 frames : + - 1 KeyFrame at origin + - 1 keyframe created by process() in voteForKeyFrame() since conditions to create a keyframe are met + - 1 frame that would be used by future data + */ + ASSERT_EQ(problem->getTrajectoryPtr()->getFrameList().size(),(unsigned int) 3); + + /* if max_time_span == 2, Wolf tree should be + + Hardware + S1 + pm5 + o: C2 - F2 + l: C4 - F3 + Trajectory + KF1 + Estim, ts=0, x = ( 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0) + C1 -> S1 + KF2 + Estim, ts=2.1, x = ( 2.2 0 -22 0 0 0 1 2.1 0 -21 0 0 0 0 0 0 ) + C2 -> S1 + f1 + m = ( 2.21 0 0 0 0 0 1 2.1 0 0 ) + c1 --> F1 + F3 + Estim, ts=2.1, x = ( . . .) + C4 -> S1 + */ + //TODO : ASSERT TESTS to make sure the constraints are correctly set + check the tree above + +} + +//replace TEST by TEST_F if SetUp() needed +TEST_F(ProcessorIMUt, interpolate) +{ + using namespace wolf; + + t.set(0); + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + data << 2, 0, 0, 0, 0, 0; // only acc_x + cap_imu_ptr->setData(data); + + // make one step to depart from origin + cap_imu_ptr->setTimeStamp(0.05); + sensor_ptr->process(cap_imu_ptr); + Motion mot_ref = problem->getProcessorMotionPtr()->getMotion(); + + // make two steps, then pretend it's just one + cap_imu_ptr->setTimeStamp(0.10); + sensor_ptr->process(cap_imu_ptr); + Motion mot_int_gt = problem->getProcessorMotionPtr()->getMotion(); + + cap_imu_ptr->setTimeStamp(0.15); + sensor_ptr->process(cap_imu_ptr); + Motion mot_final = problem->getProcessorMotionPtr()->getMotion(); + mot_final.delta_ = mot_final.delta_integr_; + Motion mot_sec = mot_final; + +// problem->getProcessorMotionPtr()->getBuffer().print(0,1,1,0); + +class P : public wolf::ProcessorIMU +{ + public: + P() : ProcessorIMU(std::make_shared<ProcessorParamsIMU>()) {} + Motion interp(const Motion& ref, Motion& sec, TimeStamp ts) + { + return ProcessorIMU::interpolate(ref, sec, ts); + } +} imu; + +Motion mot_int = imu.interp(mot_ref, mot_sec, TimeStamp(0.10)); + +ASSERT_MATRIX_APPROX(mot_int.data_, mot_int_gt.data_, 1e-6); +//ASSERT_MATRIX_APPROX(mot_int.delta_, mot_int_gt.delta_, 1e-6); // FIXME: delta_p not correctly interpolated +ASSERT_MATRIX_APPROX(mot_final.delta_integr_, mot_sec.delta_integr_, 1e-6); + +} + +TEST_F(ProcessorIMUt, acc_x) +{ + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity! + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.1); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + VectorXs x(10); + x << 0.01,0,0, 0,0,0,1, 0.2,0,0; // advanced at a=2m/s2 during 0.1s ==> dx = 0.5*2*0.1^2 = 0.01; dvx = 2*0.1 = 0.2 + Vector6s b; b << 0,0,0, 0,0,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL); +} + +TEST_F(ProcessorIMUt, acc_y) +{ + // last part of this test fails with precision wolf::Constants::EPS_SMALL beacause error is in 1e-12 + // difference hier is that we integrate over 1ms periods + + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + data << 0, 20, 9.806, 0, 0, 0; // only acc_y, but measure gravity! + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + VectorXs x(10); + x << 0,0.00001,0, 0,0,0,1, 0,0.02,0; // advanced at a=20m/s2 during 0.001s ==> dx = 0.5*20*0.001^2 = 0.00001; dvx = 20*0.001 = 0.02 + Vector6s b; b<< 0,0,0, 0,0,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL); + + //do so for 5s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + cap_imu_ptr->setTimeStamp(i*0.001 + 0.001); //first one will be 0.002 and last will be 5.000 + sensor_ptr->process(cap_imu_ptr); + } + + // advanced at a=20m/s2 during 1s ==> dx = 0.5*20*1^2 = 10; dvx = 20*1 = 20 + x << 0,10,0, 0,0,0,1, 0,20,0; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, acc_z) +{ + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + data << 0, 0, 9.806 + 2.0, 0, 0, 0; // only acc_z, but measure gravity! + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.1); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + VectorXs x(10); + x << 0,0,0.01, 0,0,0,1, 0,0,0.2; // advanced at a=2m/s2 during 0.1s ==> dz = 0.5*2*0.1^2 = 0.01; dvz = 2*0.1 = 0.2 + Vector6s b; b<< 0,0,0, 0,0,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS_SMALL); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS_SMALL); + + //do so for 5s + const unsigned int iter = 50; //how 0.1s + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + cap_imu_ptr->setTimeStamp(i*0.1 + 0.1); //first one will be 0.2 and last will be 5.0 + sensor_ptr->process(cap_imu_ptr); + } + + // advanced at a=2m/s2 during 5s ==> dz = 0.5*2*5^2 = 25; dvz = 2*5 = 10 + x << 0,0,25, 0,0,0,1, 0,0,10; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibration() , b, wolf::Constants::EPS); + ASSERT_MATRIX_APPROX(problem->getProcessorMotionPtr()->getLastPtr()->getCalibrationPreint() , b, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, check_Covariance) +{ + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + data << 2, 0, 9.806, 0, 0, 0; // only acc_x, but measure gravity! + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.1); + sensor_ptr->process(cap_imu_ptr); + + VectorXs delta_preint(problem->getProcessorMotionPtr()->getMotion().delta_integr_); +// Matrix<wolf::Scalar,9,9> delta_preint_cov = problem->getProcessorMotionPtr()->getCurrentDeltaPreintCov(); + + ASSERT_FALSE(delta_preint.isMuchSmallerThan(1, wolf::Constants::EPS_SMALL)); +// ASSERT_MATRIX_APPROX(delta_preint_cov, MatrixXs::Zero(9,9), wolf::Constants::EPS_SMALL); // << "delta_preint_cov :\n" << delta_preint_cov; //covariances computed only at keyframe creation +} + +TEST_F(ProcessorIMUt, gyro_x) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + + //do so for 5s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()); + + cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_x_biasedAbx) +{ + // time + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + + // bias + wolf::Scalar abx = 0.002; + Vector6s bias; bias << abx,0,0, 0,0,0; + Vector3s acc_bias = bias.head(3); + // state + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + + // init things + problem->setPrior(x0, P0, t, 0.01); + + problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias); + problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias); +// WOLF_DEBUG("calib: ", cap_imu_ptr->getCalibration().transpose()); + + // data + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << acc_bias - wolf::gravity(), rate_of_turn, 0, 0; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x_true(10); + x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 + + VectorXs x_est(10); + x_est = problem->getCurrentState().head(10); + + ASSERT_MATRIX_APPROX(x_est.transpose() , x_true.transpose(), wolf::Constants::EPS); + + //do so for 5s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias; + + cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + x_true << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x_true, wolf::Constants::EPS); + +} + +TEST_F(ProcessorIMUt, gyro_xy_biasedAbxy) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + wolf::Scalar abx(0.002), aby(0.01); + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + Vector6s bias; bias << abx,aby,0, 0,0,0; + Vector3s acc_bias = bias.head(3); + + problem->getProcessorMotionPtr()->getOriginPtr()->setCalibration(bias); + problem->getProcessorMotionPtr()->getLastPtr()->setCalibrationPreint(bias); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; +// data << 0+abx, 0+aby, 9.806, rate_of_turn, rate_of_turn*1.5, 0; // measure gravity + data << acc_bias - wolf::gravity(), rate_of_turn*1.5, 0, 0; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0;//, abx,aby,0, 0,0,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL);// << "expected state : " << problem->getCurrentState().transpose() +// << "\n estimated state : " << x.transpose(); + + //do so for 5s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()) + acc_bias; + + cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS);// << "estimated state is : \n" << problem->getCurrentState().transpose() << +// "\n expected state is : \n" << x.transpose() << std::endl; +} + +TEST_F(ProcessorIMUt, gyro_z) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity! + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + + //do so for 5s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 + sensor_ptr->process(cap_imu_ptr); + } + + x << 0,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_xyz) +{ + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + Vector3s tmp_vec; //will be used to store rate of turn data + Quaternions quat_comp(Quaternions::Identity()); + Matrix3s R0(Matrix3s::Identity()); + wolf::Scalar time = 0; + const unsigned int x_rot_vel = 5; + const unsigned int y_rot_vel = 6; + const unsigned int z_rot_vel = 13; + + wolf::Scalar tmpx, tmpy, tmpz; + + /* + ox oy oz evolution in degrees (for understanding) --> converted in rad + with * pi/180 + ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus + oy = pi*sin(beta*t*pi/180); + oz = pi*sin(gamma*t*pi/180); + + corresponding rate of turn + %rate of turn expressed in radians/s + wx = pi*alpha*cos(alpha*t*pi/180)*pi/180; + wy = pi*beta*cos(beta*t*pi/180)*pi/180; + wz = pi*gamma*cos(gamma*t*pi/180)*pi/180; + */ + + const wolf::Scalar dt = 0.001; + + for(unsigned int data_iter = 0; data_iter <= 1000; data_iter ++) + { + time += dt; + + tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180; + tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180; + tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180; + tmp_vec << tmpx, tmpy, tmpz; + + // quaternion composition + quat_comp = quat_comp * wolf::v2q(tmp_vec*dt); + R0 = R0 * wolf::v2R(tmp_vec*dt); + // use processorIMU + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured + data.tail(3) = tmp_vec; + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(time); + sensor_ptr->process(cap_imu_ptr); + } + + /* We focus on orientation here. position is supposed not to have moved + * we integrated using 2 ways : + - a direct quaternion composition q = q (x) q(w*dt) + - using methods implemented in processorIMU + + We check one against the other + */ + + // validating that the quaternion composition and rotation matrix composition actually describe the same rotation. + Quaternions R2quat(wolf::v2q(wolf::R2v(R0))); + Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() ); + Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() ); + + ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS);// << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl; + + VectorXs x(10); + x << 0,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 0,0,0; + + Quaternions result_quat(problem->getCurrentState().data() + 3); + //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl; + + //check position part + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS); + + //check orientation part + ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS); + + //check velocity and bias parts + ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_z_ConstVelocity) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 2,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << -wolf::gravity(), 0, 0, rate_of_turn; // measure gravity! + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002m + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + + //do so for 1s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 5.000 + sensor_ptr->process(cap_imu_ptr); + } + + x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_x_ConstVelocity) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 2,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << 0, 0, 9.806, rate_of_turn, 0, 0; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002 + x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + + //do so for 1s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()); + + cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_xy_ConstVelocity) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 2,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002 + x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + + //do so for 1s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()); + + cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_y_ConstVelocity) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 2,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << 0, 0, 9.806, 0, rate_of_turn, 0; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180, 2m/s * 0.001s = 0.002 + x << 0.002,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS_SMALL); + + //do so for 1s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 1; i < iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()); + + cap_imu_ptr->setTimeStamp(i*dt + dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + x << 2,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 2,0,0; //2m/s * 1s = 2m + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_xyz_ConstVelocity) +{ + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 2,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + Vector3s tmp_vec; //will be used to store rate of turn data + Quaternions quat_comp(Quaternions::Identity()); + Matrix3s R0(Matrix3s::Identity()); + wolf::Scalar time = 0; + const unsigned int x_rot_vel = 5; + const unsigned int y_rot_vel = 6; + const unsigned int z_rot_vel = 13; + + wolf::Scalar tmpx, tmpy, tmpz; + + /* + ox oy oz evolution in degrees (for understanding) --> converted in rad + with * pi/180 + ox = pi*sin(alpha*t*pi/180); %express angle in rad before using sinus + oy = pi*sin(beta*t*pi/180); + oz = pi*sin(gamma*t*pi/180); + + corresponding rate of turn + %rate of turn expressed in radians/s + wx = pi*alpha*cos(alpha*t*pi/180)*pi/180; + wy = pi*beta*cos(beta*t*pi/180)*pi/180; + wz = pi*gamma*cos(gamma*t*pi/180)*pi/180; + */ + + const wolf::Scalar dt = 0.001; + + for(unsigned int data_iter = 0; data_iter < 1000; data_iter ++) + { + time += dt; + + tmpx = M_PI*x_rot_vel*cos((M_PI/180) * x_rot_vel * time)*M_PI/180; + tmpy = M_PI*y_rot_vel*cos((M_PI/180) * y_rot_vel * time)*M_PI/180; + tmpz = M_PI*z_rot_vel*cos((M_PI/180) * z_rot_vel * time)*M_PI/180; + tmp_vec << tmpx, tmpy, tmpz; + + // quaternion composition + quat_comp = quat_comp * wolf::v2q(tmp_vec*dt); + R0 = R0 * wolf::v2R(tmp_vec*dt); + // use processorIMU + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()); //gravity measured + data.tail(3) = tmp_vec; + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(time); + sensor_ptr->process(cap_imu_ptr); + } + + /* We focus on orientation here. position is supposed not to have moved + * we integrated using 2 ways : + - a direct quaternion composition q = q (x) q(w*dt) + - using methods implemented in processorIMU + + We check one against the other + */ + + // validating that the quaternion composition and rotation matrix composition actually describe the same rotation. + Quaternions R2quat(wolf::v2q(wolf::R2v(R0))); + Vector4s quat_comp_vec((Vector4s() <<quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w()).finished() ); + Vector4s R2quat_vec((Vector4s() <<R2quat.x(), R2quat.y(), R2quat.z(), R2quat.w()).finished() ); + + ASSERT_MATRIX_APPROX(quat_comp_vec , R2quat_vec, wolf::Constants::EPS); // << "quat_comp_vec : " << quat_comp_vec.transpose() << "\n R2quat_vec : " << R2quat_vec.transpose() << std::endl; + + VectorXs x(10); + //rotation + translation due to initial velocity + x << 2,0,0, quat_comp.x(), quat_comp.y(), quat_comp.z(), quat_comp.w(), 2,0,0; + + Quaternions result_quat(problem->getCurrentState().data() + 3); + //std::cout << "final orientation : " << wolf::q2v(result_quat).transpose() << std::endl; + + //check position part + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(3) , x.head(3), wolf::Constants::EPS); + + //check orientation part + ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(3,4) , x.segment(3,4) , wolf::Constants::EPS); + + //check velocity + ASSERT_MATRIX_APPROX(problem->getCurrentState().segment(7,3) , x.segment(7,3), wolf::Constants::EPS); + +} + +TEST_F(ProcessorIMUt, gyro_x_acc_x) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << 1, 0, -wolf::gravity()(2), rate_of_turn, 0, 0; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on x axis + // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on x axis + // v = a*dt = 0.001 + x << 0.0000005,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0.001,0,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() << +// "\n current x is : \n" << x.transpose() << std::endl; + + //do so for 1s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 2; i <= iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<1,0,0).finished(); + + cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on x axis + // v = a*dt = 1 + x << 0.5,0,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 1,0,0; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_y_acc_y) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << 0, 1, -wolf::gravity()(2), 0, rate_of_turn, 0; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on y axis + // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on y axis + // v = a*dt = 0.001 + x << 0,0.0000005,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0.001,0; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); // << "1. current state is : \n" << problem->getCurrentState().transpose() << +// "\n current x is : \n" << x.transpose() << std::endl; + + //do so for 1s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 2; i <= iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,1,0).finished(); + + cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on y axis + // v = a*dt = 1 + x << 0,0.5,0, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,1,0; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +TEST_F(ProcessorIMUt, gyro_z_acc_z) +{ + wolf::Scalar dt(0.001); + t.set(0); // clock in 0,1 ms ticks + x0 << 0,0,0, 0,0,0,1, 0,0,0; + MatrixXs P0(9,9); P0.setIdentity(); + problem->setPrior(x0, P0, t, 0.01); + + wolf::Scalar rate_of_turn = 5 * M_PI/180.0; + data << 0, 0, -wolf::gravity()(2) + 1, 0, 0, rate_of_turn; // measure gravity + + cap_imu_ptr->setData(data); + cap_imu_ptr->setTimeStamp(0.001); + sensor_ptr->process(cap_imu_ptr); + + // Expected state after one integration + Quaternions quat_comp(Quaternions::Identity()); + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + VectorXs x(10); + // rotated at 5 deg/s for 0.001s = 0.005 deg => 0.005 * M_PI/180 on z axis + // translation with constant acc : 1 m/s^2 for 0.001 second. Initial velocity : 0, p = 0.5*a*dt^2 + v*dt = 0.5*1*0.001^2 = 0.0000005 on z axis + // v = a*dt = 0.001 + x << 0,0,0.0000005, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,0.001; + + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); + + //do so for 1s + const unsigned int iter = 1000; //how many ms + for(unsigned int i = 2; i <= iter; i++) //already did one integration above + { + // quaternion composition + quat_comp = quat_comp * wolf::v2q(data.tail(3)*dt); + + Quaternions rot(problem->getCurrentState().data()+3); + data.head(3) = rot.conjugate() * (- wolf::gravity()) + (Vector3s()<<0,0,1).finished(); + + cap_imu_ptr->setTimeStamp(i*dt); //first one will be 0.002 and last will be 1.000 + cap_imu_ptr->setData(data); + sensor_ptr->process(cap_imu_ptr); + } + + // translation with constant acc : 1 m/s for 1 second. Initial velocity : 0, p = 0.5*a*dt + v*dt = 0.5 on z axis + // v = a*dt = 1 + x << 0,0,0.5, quat_comp.x(),quat_comp.y(),quat_comp.z(),quat_comp.w(), 0,0,1; + ASSERT_MATRIX_APPROX(problem->getCurrentState().head(10) , x, wolf::Constants::EPS); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + //::testing::GTEST_FLAG(filter) = "ProcessorIMUt.check_Covariance"; + return RUN_ALL_TESTS(); +} + -- GitLab