diff --git a/CMakeLists.txt b/CMakeLists.txt index d76d259b303c034477695255eaea250a96b4b845..b6539a1bab14ae553a09626abd5611f802d157d3 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -372,6 +372,7 @@ SET(HDRS_FEATURE include/base/feature/feature_polyline_2D.h include/base/feature/feature_base.h include/base/feature/feature_match.h + include/base/feature/feature_match_polyline_2D.h include/base/feature/feature_pose.h include/base/feature/feature_gnss_fix.h include/base/feature/feature_gnss_single_diff.h @@ -626,6 +627,9 @@ IF (laser_scan_utils_FOUND) SET(HDRS_SENSOR ${HDRS_SENSOR} include/base/sensor/sensor_laser_2D.h ) + SET(SRCS_CAPTURE ${SRCS_CAPTURE} + src/capture/capture_laser_2D.cpp + ) SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} src/processor/processor_polyline.cpp src/processor/processor_tracker_feature_corner.cpp diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h index db93b3319a929aff2b6addfcbade9640690521f7..68f5d421fd99a19f695427d7bd68ca513000e2bf 100644 --- a/include/base/landmark/landmark_polyline_2D.h +++ b/include/base/landmark/landmark_polyline_2D.h @@ -253,7 +253,7 @@ inline bool LandmarkPolyline2D::isClosed() const inline LandmarkPolyline2DPtr LandmarkPolyline2D::getMergedInLandmark() const { // recursive call - if (merged_in_lmk_->getMergedInLandmark() != nullptr) + if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr) return merged_in_lmk_->getMergedInLandmark(); return merged_in_lmk_; } diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline.h index f5059b7e0f6c5b86a8746e6492733b4822a785bc..9d4e04c541c5f7533465b25df2151abaa9772966 100644 --- a/include/base/processor/processor_tracker_feature_polyline.h +++ b/include/base/processor/processor_tracker_feature_polyline.h @@ -171,27 +171,25 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature */ virtual void postProcess() override; - FeatureMatchPolyline2DList tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const; void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming, const Eigen::Matrix3s& _T_last_incoming_prior) const; -// FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A, -// const FeatureMatchPolyline2DPtr& _match_B) const; - - LandmarkMatchPolyline2DList tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const; void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const; bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr); void computeTransformations(); - //void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const; - public: /// @brief Factory method static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr _sensor_ptr); + + const FeatureBaseList& getLastNewFeatures() const + { + return all_features_last_; + } }; } /* namespace wolf */ diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp index 0b8aad1fc492fac0461cbe94d42953298ecfb7ae..e6f7c9a7a5572b0c1e2deb4bab5c8addcfdf3ff6 100644 --- a/src/processor/polyline_2D_utils.cpp +++ b/src/processor/polyline_2D_utils.cpp @@ -26,7 +26,7 @@ Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T) { Eigen::Vector3s pose; pose.head(2) = T.topRightCorner(2,1); - pose(2) = atan2(T(0,0),T(1,0)); + pose(2) = atan2(T(1,0),T(0,0)); return pose; } @@ -36,6 +36,7 @@ Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen: assert(_points_A.cols() == _points_B.cols()); assert(_points_A.rows() >= 2); assert(_points_B.rows() >= 2); + assert(_points_A.cols() >= 2); Eigen::Vector3s t_A_B; @@ -362,8 +363,8 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool const Scalar& max_sq_error ) { //std::cout << "computeBestRigidTrans...\n"; - //std::cout << "\tA is " << (_closed_A ? "CLOSED" : "OPEN") << " has "<< _points_A.cols() << " points\n"; - //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "OPEN") << " has "<< _points_B.cols() << " points\n"; + //std::cout << "\tA is " << (_closed_A ? "CLOSED" : "NOT CLOSED") << " has "<< _points_A.cols() << " points\n"; + //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "NOT CLOSED") << " has "<< _points_B.cols() << " points\n"; //std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl; //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl; @@ -393,7 +394,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool //std::cout << "Auto-calling switching A<->B...\n"; matches = computeBestRigidTrans(_points_B, _first_defined_B, _last_defined_B, _closed_B, - _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error); + _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error); // undo switching for (auto match : matches) { @@ -472,8 +473,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool //std::cout << "\t\tto_A_defined " << to_A_defined << (to_A == 0) << (to_A == last_A) << _last_defined_A << std::endl; //std::cout << "\t\tto_B_defined " << to_B_defined << (to_B == 0) << (to_B == last_B) << _last_defined_B << std::endl; - // If only one overlapped point, both should be defined - if ( N_overlapped== 1 && + // If only one overlapped point, no rigid transformation possible + if ( N_overlapped == 1 ) + continue; + // If only two overlapped points, all should be defined + if ( N_overlapped == 2 && (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) ) continue; @@ -489,6 +493,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool assert(to_B < _points_B.cols()); // RIGID TRANSFORMATION FOR DEFINED POINTS + //std::cout << "\tcomputing rigid transformation...\n"; int from_B_def = from_B_defined ? from_B : from_B+1; int from_A_def = from_A_defined ? from_A : from_A+1; int to_A_def = to_A_defined ? to_A : to_A+1; @@ -502,7 +507,9 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool points_A_def.leftCols(_points_A.cols()-from_A_def) = _points_A.middleCols(from_A_def, _points_A.cols()-from_A_def); points_A_def.rightCols(to_A_def+1) = _points_A.middleCols(0, to_A_def+1); } + //std::cout << "\tpoints filled...\n"; + //std::cout << "\tcomputeRigidTrans\n"; Eigen::Vector3s pose_A_B = computeRigidTrans(points_A_def, points_B_def); Eigen::Matrix3s T_A_B = pose2T2D(pose_A_B.head(2), pose_A_B(2)); @@ -512,6 +519,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool dist2.middleRows(from_B_defined && from_A_defined ? 0 : 1, N_overlapped_def) = d.colwise().squaredNorm().transpose().array(); // POINT-LINE DISTANCES for not defined extremes + //std::cout << "\tPOINT-LINE DISTANCES\n"; // First if (!from_B_defined || !from_A_defined) { @@ -546,7 +554,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool to_B_defined); assert(N_overlapped > 1); } - //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl; + //std::cout << "\t\tsquared distances = " << dist2.transpose() << "( should be < " << max_sq_error << ")" << std::endl; // All squared distances should be within a threshold if ((dist2 < max_sq_error).all()) @@ -562,9 +570,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool // Store matches ordered by the squared norm of the transformation (I know, mixing mts and rads..) matches.push_back(match); + std::cout << "match added to list of matches!\n"; } } + std::cout << matches.size() << " rigid transformations found\n"; return matches; } diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline.cpp index a2198720c41eb5cfabcf12465a6e43568918b330..29c688b9333f8484a1c1e61708a87bb3518d9252 100644 --- a/src/processor/processor_tracker_feature_polyline.cpp +++ b/src/processor/processor_tracker_feature_polyline.cpp @@ -27,6 +27,11 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis FeatureBaseList& _features_incoming_out, FeatureMatchMap& _feature_correspondences) { + WOLF_DEBUG("PTF ", getName(), "::trackFeatures", _features_last_in.size()); + + if (_features_last_in.empty()) + return 0; + // prior transformations Eigen::Matrix3s T_last_incoming_prior(Eigen::Matrix3s::Identity()); T_last_incoming_prior.topLeftCorner(2,2) = R_last_incoming_; @@ -42,58 +47,74 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); FeatureMatchPolyline2DScalarMap best_matches; + std::cout << "\tmatching feature incoming " << pl_incoming->id() << "\n"; + // Check matching with all features in last // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior for (auto ftr_last_ : _features_last_in) tryMatchWithFeature(best_matches, std::static_pointer_cast<FeaturePolyline2D>(ftr_last_), pl_incoming, T_last_incoming_prior); + std::cout << "\t" << best_matches.size() << " matches with features last found\n"; + // Try the match for all candidates (sorted by difference from movement prior) for (auto best_ftr_match_pair : best_matches) { auto best_ftr_match = best_ftr_match_pair.second; auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(best_ftr_match->feature_ptr_); - auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[pl_last]->landmark_ptr_); - - // the match with this last_ feature was removed after any updateMatchWithLandmark - if(landmark_match_map_.find(pl_last) == landmark_match_map_.end()) - continue; - auto lmk_match_last = landmark_match_map_[pl_last]; + std::cout << "\t\tconfirming match with feature last: " << pl_last->id() << "\n"; - // Landmark changed or was merged -> Redo match with last feature - if (lmk_match_last->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr) + // There is a landmark_match (i.e. it has not been recently created in processNew) + if(find(new_features_last_.begin(), new_features_last_.end(), pl_last) == new_features_last_.end()) { - if (!updateMatchWithLandmark(lmk_match_last,pl_lmk,pl_last)) + // the match with this last_ feature was removed after any updateMatchWithLandmark + if(landmark_match_map_.find(pl_last) == landmark_match_map_.end()) + { + std::cout << "\t\tremoved feature last\n"; + continue; + } + + auto lmk_match_last = landmark_match_map_[pl_last]; + auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match_last->landmark_ptr_); + + // Landmark changed or was merged -> Redo match with last feature + if (lmk_match_last->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr) + { + std::cout << "\t\tupdating match\n"; + if (!updateMatchWithLandmark(lmk_match_last,pl_lmk,pl_last)) + continue; + } + // removed -> not valid match with landmark + else if (pl_lmk->isRemoving()) continue; + + // Add the match to landmark_match_map + auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>(); + + lmk_match_incoming->feature_from_id_ = best_ftr_match->feature_incoming_from_id_; + lmk_match_incoming->feature_to_id_ = best_ftr_match->feature_incoming_to_id_; + // Not adding new points nor checks (done in correctFeatureDrift) + lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_ + best_ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_; + lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_ + best_ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_; + lmk_match_incoming->landmark_ptr_ = pl_lmk; + lmk_match_incoming->landmark_version_ = pl_lmk->getVersion(); + lmk_match_incoming->normalized_score_ = best_ftr_match->normalized_score_; + lmk_match_incoming->T_feature_landmark_ = best_ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_; + + landmark_match_map_[pl_incoming] = lmk_match_incoming; } - // removed -> not valid match with landmark - else if (pl_lmk->isRemoving()) - continue; - // TRACK still valid + // TRACK valid + std::cout << "\t\tvalid match, storing...\n"; // add best match to match_map _feature_correspondences[feature_out_ptr] = feature_in_ptr _feature_correspondences[pl_incoming] = best_ftr_match; // add feature to list of tracked features _features_incoming_out.push_back(pl_incoming); - // Add the match to landmark_match_map - auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>(); - - lmk_match_incoming->feature_from_id_ = best_ftr_match->feature_incoming_from_id_; - lmk_match_incoming->feature_to_id_ = best_ftr_match->feature_incoming_to_id_; - // Not adding new points nor checks (done in correctFeatureDrift) - lmk_match_incoming->landmark_from_id_ = lmk_match_last->landmark_from_id_ + best_ftr_match->feature_last_from_id_ - lmk_match_last->feature_from_id_; - lmk_match_incoming->landmark_to_id_ = lmk_match_last->landmark_to_id_ + best_ftr_match->feature_last_to_id_ - lmk_match_last->feature_to_id_; - lmk_match_incoming->landmark_ptr_ = pl_lmk; - lmk_match_incoming->landmark_version_ = pl_lmk->getVersion(); - lmk_match_incoming->normalized_score_ = best_ftr_match->normalized_score_; - lmk_match_incoming->T_feature_landmark_ = best_ftr_match->T_incoming_last_ * lmk_match_last->T_feature_landmark_; - - landmark_match_map_[pl_incoming] = lmk_match_incoming; - // match for this feature has been found matched = true; + std::cout << "\t\tmatch found\n"; break; } @@ -103,6 +124,8 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis else ftr_it++; } + + WOLF_DEBUG(_feature_correspondences.size(), " features tracked"); return _feature_correspondences.size(); } @@ -110,6 +133,7 @@ bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _ const FeatureBasePtr _last_feature, FeatureBasePtr _incoming_feature) { + WOLF_DEBUG("PTF ", getName(), "::correctFeatureDrift: "); // If incoming observed new landmark points & landmark has points // Check if match is still valid and update the match // TODO @@ -123,6 +147,7 @@ bool ProcessorTrackerFeaturePolyline::correctFeatureDrift(const FeatureBasePtr _ bool ProcessorTrackerFeaturePolyline::voteForKeyFrame() { + WOLF_DEBUG("PTF ", getName(), "::voteForKeyFrame: "); // TODO /* IDEAS: * - Any feature incoming derived too much from projected Landmark @@ -135,8 +160,11 @@ bool ProcessorTrackerFeaturePolyline::voteForKeyFrame() unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _max_features) { + WOLF_DEBUG("PTF ", getName(), "::processNew: "); unsigned int n = ProcessorTrackerFeature::processNew(_max_features); + WOLF_DEBUG("Processing ", n, " new last features"); + // prior transformations Eigen::Matrix3s T_sensor_world_last(Eigen::Matrix3s::Identity()); T_sensor_world_last.topLeftCorner(2,2) = R_sensor_world_last_; @@ -149,6 +177,7 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma { auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); LandmarkMatchPolyline2DScalarMap best_lmk_matches; + WOLF_DEBUG("Processing feature: ", pl_ftr->id()); // Check matching with all existing polyline landmarks // Store all matches consistent with T_last_incoming_prior in best_matches sorted by difference from T_last_incoming_prior @@ -202,19 +231,26 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned int& _max_new_features, FeatureBaseList& _features_last_out) { + WOLF_DEBUG("PTF ", getName(), "::detectNewFeatures (", all_features_last_.size(), " in all_features_last_)"); int prev_size = _features_last_out.size(); - int N_new_features = new_features_last_.size(); if (_features_last_out == new_features_last_) { + WOLF_DEBUG("_features_last_out:", _features_last_out.size(), " before splice"); + WOLF_DEBUG("all_features_last_:", all_features_last_.size(), " before splice"); _features_last_out.splice(_features_last_out.end(), all_features_last_); + WOLF_DEBUG("_features_last_out:", _features_last_out.size(), " after splice"); + WOLF_DEBUG("all_features_last_:", all_features_last_.size(), " after splice"); - if (_features_last_out.size() > prev_size + _max_new_features) + // 0 means unlimited new features + if (_max_new_features != 0 && _features_last_out.size() > prev_size + _max_new_features) { _features_last_out.resize(prev_size + _max_new_features); + WOLF_DEBUG(_max_new_features, " were provided (max new features)"); return _max_new_features; } - return N_new_features; + WOLF_DEBUG(_features_last_out.size()-prev_size, " were provided"); + return _features_last_out.size()-prev_size; } else std::runtime_error("asking for new features not from last"); @@ -224,23 +260,29 @@ unsigned int ProcessorTrackerFeaturePolyline::detectNewFeatures(const unsigned i void ProcessorTrackerFeaturePolyline::establishConstraints() { + WOLF_DEBUG("PTF ", getName(), "::establishConstraints: "); unsigned int N_constraints = 0; // Create a constraint for each match in last features auto ftr_it = last_ptr_->getFeatureList().begin(); while (ftr_it != last_ptr_->getFeatureList().end()) { + WOLF_DEBUG("\tLast feature: ", (*ftr_it)->id()); assert(landmark_match_map_.find(*ftr_it) != landmark_match_map_.end() && "feature without landmark match in last features"); - auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[*ftr_it]->landmark_ptr_); - auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); auto lmk_match = landmark_match_map_[*ftr_it]; + auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_); + auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it); + WOLF_DEBUG("\tLandmark: ", pl_lmk->id()); // LANDMARK CHANGED: update match + WOLF_DEBUG("\tLandmark changed?"); if (lmk_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr) { + WOLF_DEBUG("\tLandmark changed"); // not successful update if (!updateMatchWithLandmark(lmk_match,pl_lmk,pl_ftr)) { + WOLF_DEBUG("\tNot successful update"); // remove from match map landmark_match_map_.erase(*ftr_it); // remove from last feature list @@ -250,22 +292,29 @@ void ProcessorTrackerFeaturePolyline::establishConstraints() } // LANDMARK REMOVED: remove match and feature - else if (pl_lmk->isRemoving()) + else { - // remove from match map - landmark_match_map_.erase(*ftr_it); - // remove from last feature list - ftr_it = last_ptr_->getFeatureList().erase(ftr_it); - continue; + WOLF_DEBUG("\tLandmark removed?"); + if (pl_lmk->isRemoving()) + { + WOLF_DEBUG("\tLandmark was removed"); + // remove from match map + landmark_match_map_.erase(*ftr_it); + // remove from last feature list + ftr_it = last_ptr_->getFeatureList().erase(ftr_it); + continue; + } } // MODIFY LANDMARK (only for not closed and not recently created) + WOLF_DEBUG("\tModifying.."); if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0) // If modified, add lmk to modified_lmks_ list if (modifyLandmarkAndMatch(lmk_match, pl_ftr)) modified_lmks_.push_back(pl_lmk); // ESTABLISH CONSTRAINTS + WOLF_DEBUG("\tEstablishing constraints.."); 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"); @@ -281,46 +330,46 @@ void ProcessorTrackerFeaturePolyline::establishConstraints() int lmk_point_id = lmk_match->landmark_from_id_; for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++) { - //std::cout << "feature point " << ftr_point_id << std::endl; - //std::cout << "landmark point " << lmk_point_id << std::endl; + std::cout << "feature point " << ftr_point_id << std::endl; + std::cout << "landmark point " << lmk_point_id << std::endl; // First not defined point if (ftr_point_id == 0 && !pl_ftr->isFirstDefined()) // first point to line constraint { - //std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl; + std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getNextValidId(lmk_point_id) << std::endl; assert(!(lmk_point_id == pl_lmk->getLastId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for first not defined point of a feature with the last landmark point without being closed. A landmark didn't grow correctly."); // emplace constraint emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getNextValidId(lmk_point_id)); N_constraints++; - //std::cout << "constraint added" << std::endl; + std::cout << "constraint added" << std::endl; } // Last not defined point else if (ftr_point_id == pl_ftr->getNPoints()-1 && !pl_ftr->isLastDefined()) // last point to line constraint { - //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl; + std::cout << "point-line: landmark points " << lmk_point_id << ", " << pl_lmk->getPrevValidId(lmk_point_id) << std::endl; assert(!(lmk_point_id == pl_lmk->getFirstId() && !pl_lmk->isClosed()) && "Trying to establish a constraint for last not defined point of a feature with the first landmark point without being closed. A landmark didn't grow correctly."); // emplace constraint emplaceConstraintPointToLine(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id, pl_lmk->getPrevValidId(lmk_point_id)); N_constraints++; - //std::cout << "constraint added" << std::endl; + std::cout << "constraint added" << std::endl; } // Defined point else // point to point constraint { - //std::cout << "point-point: landmark point " << lmk_point_id << std::endl; - //std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl; - //std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl; - //std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl; + std::cout << "point-point: landmark point " << lmk_point_id << std::endl; + std::cout << "landmark first id:" << pl_lmk->getFirstId() << std::endl; + std::cout << "landmark last id:" << pl_lmk->getLastId() << std::endl; + std::cout << "landmark n points:" << pl_lmk->getNPoints() << std::endl; emplaceConstraintPoint(pl_ftr, pl_lmk, ftr_point_id, lmk_point_id); N_constraints++; - //std::cout << "constraint added" << std::endl; + std::cout << "constraint added" << std::endl; } // Next lmk point @@ -330,7 +379,7 @@ void ProcessorTrackerFeaturePolyline::establishConstraints() // next ftr ftr_it++; } - //std::cout << N_constraints << " constraints established, trying to close and classify " << modified_lmks.size() << std::endl; + std::cout << N_constraints << " constraints established" << std::endl; } void ProcessorTrackerFeaturePolyline::emplaceConstraintPointToLine(FeaturePolyline2DPtr _polyline_feature, @@ -364,11 +413,11 @@ void ProcessorTrackerFeaturePolyline::emplaceConstraintPoint(FeaturePolyline2DPt LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _feature_ptr) { - //std::cout << "ProcessorTrackerFeaturePolyline::createLandmark" << std::endl; + WOLF_DEBUG("PTF ", getName(), "::createLandmark: "); //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl; //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl; assert(_feature_ptr->getType() == "POLYLINE 2D"); - assert(std::find(new_features_last_.begin(),new_features_last_.end(),_feature_ptr) != new_features_last_.end() && "creating a landmark for a feature which is not in last capture"); + assert(std::find(last_ptr_->getFeatureList().begin(),last_ptr_->getFeatureList().end(),_feature_ptr) != last_ptr_->getFeatureList().end() && "creating a landmark for a feature which is not in last capture"); FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr); @@ -391,6 +440,7 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyline2DPtr& lmk_match, FeaturePolyline2DPtr& pl_ftr) { + WOLF_DEBUG("PTF ", getName(), "::modifyLandmarkAndMatch: "); auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(lmk_match->landmark_ptr_); bool print = false; @@ -537,43 +587,51 @@ bool ProcessorTrackerFeaturePolyline::modifyLandmarkAndMatch(LandmarkMatchPolyli void ProcessorTrackerFeaturePolyline::advanceDerived() { + WOLF_DEBUG("PTF ", getName(), "::advanceDerived: "); // remove last features from landmark_match_map_ - for (auto ftr : last_ptr_->getFeatureList()) - { - assert(landmark_match_map_.find(ftr) != landmark_match_map_.end()); - landmark_match_map_.erase(ftr); - } + if (last_ptr_ != nullptr) + for (auto ftr : last_ptr_->getFeatureList()) + { + assert(landmark_match_map_.find(ftr) != landmark_match_map_.end()); + landmark_match_map_.erase(ftr); + } + WOLF_DEBUG("removing ", all_features_last_.size() , " features in last"); all_features_last_.clear(); all_features_last_.splice(all_features_last_.end(),all_features_incoming_); + WOLF_DEBUG("last has ", all_features_last_.size() , " features (prev. incoming)"); ProcessorTrackerFeature::advanceDerived(); } void ProcessorTrackerFeaturePolyline::resetDerived() { + WOLF_DEBUG("PTF ", getName(), "::resetDerived: "); // remove last features from landmark_match_map_ - for (auto ftr : last_ptr_->getFeatureList()) - { - assert(landmark_match_map_.find(ftr) != landmark_match_map_.end()); - landmark_match_map_.erase(ftr); - } + if (last_ptr_ != nullptr) + for (auto ftr : last_ptr_->getFeatureList()) + { + assert(landmark_match_map_.find(ftr) != landmark_match_map_.end()); + landmark_match_map_.erase(ftr); + } + WOLF_DEBUG("removing ", all_features_last_.size() , " features in last"); all_features_last_.clear(); all_features_last_.splice(all_features_last_.end(),all_features_incoming_); + WOLF_DEBUG("last has ", all_features_last_.size() , " features (prev. incoming)"); ProcessorTrackerFeature::resetDerived(); } void ProcessorTrackerFeaturePolyline::preProcess() { + WOLF_DEBUG("PTF ", getName(), "::extractPolylines: "); // Extract all polylines from incoming_ - //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::extractPolylines: "); // TODO: sort corners by bearing std::list<laserscanutils::Polyline> polylines; line_finder_.findPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_)->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensorPtr()))->getScanParams(), polylines); - //WOLF_DEBUG("Polylines extracted: ", polylines.size()); + WOLF_DEBUG("Polylines extracted: ", polylines.size()); for (auto&& pl : polylines) { @@ -584,11 +642,13 @@ void ProcessorTrackerFeaturePolyline::preProcess() } // compute transformations - computeTransformations(); + if (last_ptr_ != nullptr && incoming_ptr_ != nullptr) + computeTransformations(); } void ProcessorTrackerFeaturePolyline::postProcess() { + WOLF_DEBUG("PTF ", getName(), "::postProcess: "); // Try to close & classify modified landmarks while (!modified_lmks_.empty()) { @@ -629,178 +689,12 @@ void ProcessorTrackerFeaturePolyline::postProcess() } } -FeatureMatchPolyline2DList ProcessorTrackerFeaturePolyline::tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I) const -{ - FeatureMatchPolyline2DList ftr_matches; - - // compute best sq distance matching - MatchPolyline2DList matches = computeBestRigidTrans(_ftr_I->getPoints(), // <--feature incoming points - _ftr_I->isFirstDefined(), - _ftr_I->isLastDefined(), - false, // <--feature is not closed for sure - _ftr_L->getPoints(), // <--feature last points - _ftr_L->isFirstDefined(), - _ftr_L->isLastDefined(), - false, // <--feature is not closed for sure - params_tracker_feature_polyline_->match_alignment_position_sq_norm_th); - // valid match - for (auto match : matches) - { - auto ftr_match = std::make_shared<FeatureMatchPolyline2D>(); - // feature incoming point id's - ftr_match->feature_incoming_from_id_ = match->from_A_id_; - ftr_match->feature_incoming_to_id_ = match->to_A_id_; - // feature last point id's - ftr_match->feature_last_from_id_ = match->from_B_id_; - ftr_match->feature_last_to_id_ = match->to_B_id_; - // incoming last transformation - ftr_match->T_incoming_last_ = match->T_A_B_; - // score - ftr_match->normalized_score_ = match->normalized_score_; - // feature correspondence - ftr_match->feature_ptr_ = _ftr_L; - // store in list - ftr_matches.push_back(ftr_match); - } - - return ftr_matches; - - /* - //Search along all possible overlapping configurations between A&B points - // We define offset as the correspondence (if exists) of the first point_L to B's. - // Offset is within [-last_I, last_L] - - bool print = true; - - int from_L, to_L, from_I, to_I, N_overlapped; - int first_L = 0;//(_ftr_L->isFirstDefined() ? 0 : 1); - int first_I = 0;//(_ftr_I->isFirstDefined() ? 0 : 1); - int last_L = _ftr_L->getNPoints() - 1;//(_ftr_L->isLastDefined() ? _ftr_L->getNPoints() - 1 : _ftr_L->getNPoints() - 2); - int last_I = _ftr_I->getNPoints() - 1;//(_ftr_I->isLastDefined() ? _ftr_I->getNPoints() - 1 : _ftr_I->getNPoints() - 2); - - Eigen::MatrixXs points_L = _ftr_L->getPoints(); - Eigen::MatrixXs points_I = _ftr_I->getPoints(); - - if (print) - { - std::cout << "ProcessorTrackerFeaturePolyline::computeBestMatch...\n"; - std::cout << "\tdefined points_L:\n" << points_L.topRows(2) << std::endl; - std::cout << "\tdefined points_I:\n" << points_I.topRows(2) << std::endl; - } - - FeatureMatchPolyline2DPtr best_match = nullptr; - - // Check all overlapping configurations - int min_offset = -last_L+first_I; - int max_offset = last_I-first_L; - for (int offset = min_offset; offset <= max_offset; offset++) - { - from_L = std::max(first_L,-offset); - from_I = std::max(first_I, offset); - - N_overlapped = std::min(last_I - from_I, last_L - from_L)+1; - - to_L = from_L+N_overlapped-1; - to_I = from_I+N_overlapped-1; - - if (print) - { - std::cout << "\toffset " << offset << std::endl; - std::cout << "\t\tfrom_L " << from_L << std::endl; - std::cout << "\t\tto_L " << to_L << std::endl; - std::cout << "\t\tfrom_I " << from_I << std::endl; - std::cout << "\t\tto_I " << to_I << std::endl; - std::cout << "\t\tlast_L " << last_L << std::endl; - std::cout << "\t\tlast_I " << last_I << std::endl; - std::cout << "\t\tN_overlapped " << N_overlapped << std::endl; - } - bool from_L_defined = ((from_L != 0 && from_L != last_L) || _ftr_L->isFirstDefined()); - bool from_I_defined = ((from_I != 0 && from_I != last_I) || _ftr_I->isFirstDefined()); - bool to_L_defined = ((to_L != 0 && to_L != last_L) || _ftr_L->isLastDefined()); - bool to_I_defined = ((to_I != 0 && to_I != last_I) || _ftr_I->isLastDefined()); - - // If only one overlapped point, both should be defined - if ( N_overlapped== 1 && - (!from_L_defined || !from_I_defined || !to_L_defined || !to_I_defined ) ) - continue; - - assert(N_overlapped <= points_I.cols()); - assert(N_overlapped <= points_L.cols()); - assert(N_overlapped > 0); - assert(from_L >= 0); - assert(from_I >= 0); - assert(to_L <= last_L); - assert(to_I <= last_I); - assert(to_L < points_L.cols()); - assert(to_I < points_I.cols()); - assert(to_I == last_I || to_L == last_L); - assert(from_I == first_I || from_L == first_L); - - // POINT-POINT DISTANCES for all overlapped points - Eigen::ArrayXXd d = (points_I.block(0,from_I, 2, N_overlapped) - points_L.block(0,from_L, 2, N_overlapped)).array(); - Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2); - - // POINT-LINE DISTANCES for not defined extremes - // First - if (!from_I_defined || !from_L_defined) - { - if (print) - std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl; - - // std::cout << "\t\t\tL " << points_L.col(from_L).transpose() << std::endl; - // std::cout << "\t\t\tLaux" << points_L.col(from_L+1).transpose() << std::endl; - // std::cout << "\t\t\tI " << points_I.col(from_I).transpose() << std::endl; - dist2(0) = sqDistPointToLine(points_L.col(from_L), - points_L.col(from_L+1), - points_I.col(from_I), - from_L_defined, - from_I_defined); - assert(N_overlapped > 1); - } - // Last - if (!to_I_defined || !to_L_defined) - { - if (print) - std::cout << "\t\t\tLast feature not defined distance to line" << std::endl; - - // std::cout << "\t\t\tL " << points_L.col(to_L).transpose() << std::endl; - // std::cout << "\t\t\tLaux" << points_L.col(to_L-1).transpose() << std::endl; - // std::cout << "\t\t\tI " << points_I.col(to_I).transpose() << std::endl; - dist2(N_overlapped-1) = sqDistPointToLine(points_L.col(to_L), - points_L.col(to_L-1), - points_I.col(to_I), - to_L_defined, - to_I_defined); - assert(N_overlapped > 1); - } - if (print) - std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl; - - // All squared distances should be within a threshold - if ((dist2 < params_tracker_feature_polyline_->match_position_error_th*params_tracker_feature_polyline_->match_position_error_th).all()) - { - FeatureMatchPolyline2DPtr current_match; - current_match->feature_incoming_from_id_= from_I; - current_match->feature_incoming_to_id_= to_I; - current_match->feature_last_from_id_= from_L; - current_match->feature_last_to_id_= to_L; - current_match->feature_ptr_= _ftr_L; - current_match->normalized_score_ = dist2.mean(); - - // Choose the most overlapped one - best_match = bestFeatureMatch(best_match,current_match); - - if (print && best_match == current_match) - std::cout << "\tBEST MATCH" << std::endl; - } - } - - return best_match; - */ -} - void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I, const Eigen::Matrix3s& _T_last_incoming_prior) const { + WOLF_DEBUG("PTF ", getName(), "::tryMatchWithFeature: "); + WOLF_DEBUG("last incomming pose ", T2pose2D(_T_last_incoming_prior).transpose()); + WOLF_DEBUG("incomming last pose ", T2pose2D(_T_last_incoming_prior.inverse()).transpose()); + // compute best sq distance matching MatchPolyline2DList matches = computeBestRigidTrans(_ftr_I->getPoints(), // <--feature incoming points _ftr_I->isFirstDefined(), @@ -814,10 +708,15 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2D // valid match for (auto match : matches) { + WOLF_DEBUG("match with pose ", T2pose2D(match->T_A_B_).transpose()); Scalar sq_norm = T2pose2D(_T_last_incoming_prior * match->T_A_B_).squaredNorm(); if (sq_norm > params_tracker_feature_polyline_->match_feature_pose_sq_norm_th) + { + WOLF_DEBUG("match with squared norm ", sq_norm, " discarded"); continue; + } + WOLF_DEBUG("match with squared norm ", sq_norm, " stored!"); auto ftr_match = std::make_shared<FeatureMatchPolyline2D>(); // feature incoming point id's @@ -837,72 +736,12 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithFeature(FeatureMatchPolyline2D } } -//wolf::FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A, const FeatureMatchPolyline2DPtr& _match_B) const -//{ -// assert(_match_A != nullptr || _match_B != nullptr); -// if (_match_A == nullptr) -// return _match_B; -// -// if (_match_B == nullptr) -// return _match_A; -// -// // Choose the most overlapped match, if equally overlapped: the best score -// int overlapping_A = _match_A->feature_incoming_to_id_ - _match_A->feature_incoming_from_id_; -// int overlapping_B = _match_B->feature_incoming_to_id_ - _match_B->feature_incoming_from_id_; -// if (overlapping_A > overlapping_B || -// (overlapping_A == overlapping_B && _match_A->normalized_score_ > _match_B->normalized_score_)) -// return _match_A; -// -// return _match_B; -//} - - -LandmarkMatchPolyline2DList ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const -{ - LandmarkMatchPolyline2DList lmk_matches; - - // compute best sq distance matching - MatchPolyline2DList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(), // <--landmark points in local coordinates - _lmk_ptr->isFirstDefined(), - _lmk_ptr->isLastDefined(), - _lmk_ptr->isClosed(), - _feat_ptr->getPoints(), // <--feature points - _feat_ptr->isFirstDefined(), - _feat_ptr->isLastDefined(), - false, // <--feature is not closed for sure - params_tracker_feature_polyline_->match_alignment_position_sq_norm_th); - // valid match - for (auto match : matches) - { - auto lmk_match = std::make_shared<LandmarkMatchPolyline2D>(); - - // landmark - lmk_match->landmark_ptr_=_lmk_ptr; - lmk_match->landmark_version_=_lmk_ptr->getVersion(); - // landmark point id's - std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds(); - lmk_match->landmark_from_id_ = lmk_valid_ids[match->from_A_id_]; - lmk_match->landmark_to_id_ = lmk_valid_ids[match->to_A_id_]; - // feature point id's - lmk_match->feature_from_id_ = match->from_B_id_; - lmk_match->feature_to_id_ = match->to_B_id_; - - // incoming last transformation - lmk_match->T_feature_landmark_ = match->T_A_B_.inverse(); - // score - lmk_match->normalized_score_ = match->normalized_score_; - - assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed()); - assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed()); - - // store in list - lmk_matches.push_back(lmk_match); - } - return lmk_matches; -} - -void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const +void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, + const LandmarkPolyline2DPtr& _lmk_ptr, + const FeaturePolyline2DPtr& _feat_ptr, + const Eigen::Matrix3s& _T_feature_landmark_prior) const { + WOLF_DEBUG("PTF ", getName(), "::tryMatchWithLandmark: "); // compute best sq distance matching MatchPolyline2DList matches = computeBestRigidTrans(_lmk_ptr->computePointsGlobal(), // <--landmark points in local coordinates _lmk_ptr->isFirstDefined(), @@ -949,6 +788,7 @@ void ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(LandmarkMatchPolyline bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr) { + WOLF_DEBUG("PTF ", getName(), "::updateMatchWithLandmark: "); assert(_lmk_ptr->id() == _lmk_match->landmark_ptr_->id()); // merged: update the pl_lmk pointer @@ -959,55 +799,29 @@ bool ProcessorTrackerFeaturePolyline::updateMatchWithLandmark(LandmarkMatchPolyl } // try match with _feat_ptr again - auto new_lmk_matches = tryMatchWithLandmark(_lmk_ptr,_feat_ptr); - - // choose the one with smallest difference from movement of the old match - LandmarkMatchPolyline2DPtr best_lmk_match = nullptr; - Scalar best_sq_norm = params_tracker_feature_polyline_->match_landmark_pose_sq_norm_th; - for (auto new_lmk_match : new_lmk_matches) - { - Scalar sq_norm = T2pose2D(_lmk_match->T_feature_landmark_.inverse() * new_lmk_match->T_feature_landmark_).squaredNorm(); - if (sq_norm < best_sq_norm) - { - best_lmk_match = new_lmk_match; - best_sq_norm = sq_norm; - } - } - - _lmk_match = best_lmk_match; + LandmarkMatchPolyline2DScalarMap new_lmk_matches; + tryMatchWithLandmark(new_lmk_matches,_lmk_ptr,_feat_ptr, _lmk_match->T_feature_landmark_); // not valid match - if (_lmk_match == nullptr) + if (new_lmk_matches.empty()) { + _lmk_match = nullptr; landmark_match_map_.erase(_feat_ptr); return false; } // valid match: update landmark match of last - landmark_match_map_[_feat_ptr] = best_lmk_match; + _lmk_match = new_lmk_matches.begin()->second; // the frist is the one with smallest difference from movement of the old match + landmark_match_map_[_feat_ptr] = _lmk_match; return true; } // MATH /////////////////////////////////////////////////////////////////////////////////////////////////////////// /////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -/*void ProcessorTrackerFeaturePolyline::expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const -{ - assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerFeaturePolyline::expectedFeature: Bad landmark type"); - LandmarkPolyline2DPtr pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr); - - //std::cout << "ProcessorPolyline::expectedFeature" << std::endl; - //std::cout << "t_world_sensor_last_: " << t_world_sensor_last_.transpose() << std::endl; - //std::cout << "R_sensor_world_last_: " << std::endl << R_sensor_world_last_ << std::endl; - - expected_feature_ = pl_lmk->computePointsGlobal(); - expected_feature_.topRows(2) = R_sensor_world_last_ * (expected_feature_.topRows(2).colwise() - t_world_sensor_last_); - expected_feature_cov_ = pl_lmk->computePointsCovGlobal(); - // TODO Rotate covariances -}*/ - void ProcessorTrackerFeaturePolyline::computeTransformations() { + //WOLF_DEBUG("ProcessorTrackerFeaturePolyline::computeTransformations: "); Eigen::Vector3s vehicle_pose_incoming = getProblem()->getState(incoming_ptr_->getTimeStamp()); Eigen::Vector3s vehicle_pose_last = getProblem()->getState(last_ptr_->getTimeStamp()); @@ -1052,13 +866,16 @@ void ProcessorTrackerFeaturePolyline::computeTransformations() R_last_incoming_ = R_world_sensor_last_.transpose() * R_world_sensor_incoming_; t_last_incoming_ = t_sensor_world_last_ + R_sensor_world_last_ * t_world_sensor_incoming_; - //std::cout << "t_world_robot_incoming_ " << t_world_robot_.transpose() << std::endl; - //std::cout << "t_robot_sensor_incoming_ " << t_robot_sensor_.transpose() << std::endl; - //std::cout << "R_robot_sensor_incoming_ " << std::endl << R_robot_sensor_ << std::endl; - //std::cout << "t_world_sensor_incoming_ " << t_world_sensor_.transpose() << std::endl; - //std::cout << "R_world_sensor_incoming_ " << std::endl << R_world_sensor_ << std::endl; - //std::cout << "t_sensor_world_incoming_ " << t_sensor_world_.transpose() << std::endl; - //std::cout << "R_sensor_world_incoming_ " << std::endl << R_sensor_world_ << std::endl; + //std::cout << "t_robot_sensor_ " << t_robot_sensor_.transpose() << std::endl; + //std::cout << "R_robot_sensor_ " << std::endl << R_robot_sensor_ << std::endl; + //std::cout << "t_world_sensor_incoming_ " << t_world_sensor_incoming_.transpose() << std::endl; + //std::cout << "R_world_sensor_incoming_ " << std::endl << R_world_sensor_incoming_ << std::endl; + //std::cout << "t_sensor_world_incoming_ " << t_sensor_world_incoming_.transpose() << std::endl; + //std::cout << "R_sensor_world_incoming_ " << std::endl << R_sensor_world_incoming_ << std::endl; + //std::cout << "t_world_sensor_last_ " << t_world_sensor_last_.transpose() << std::endl; + //std::cout << "R_world_sensor_last_ " << std::endl << R_world_sensor_last_ << std::endl; + //std::cout << "t_sensor_world_last_ " << t_sensor_world_last_.transpose() << std::endl; + //std::cout << "R_sensor_world_last_ " << std::endl << R_sensor_world_last_ << std::endl; //std::cout << "t_incoming_last_ " << t_incoming_last_.transpose() << std::endl; //std::cout << "R_incoming_last_ " << std::endl << R_incoming_last_ << std::endl; //std::cout << "t_last_incoming_ " << t_last_incoming_.transpose() << std::endl; diff --git a/test/gtest_polyline_2D.cpp b/test/gtest_polyline_2D.cpp index d3c96443811118c8e3e882f2d6eb7473bb25bb52..d8c50e392e2551fd72323d8cb9c259e954ee9c4a 100644 --- a/test/gtest_polyline_2D.cpp +++ b/test/gtest_polyline_2D.cpp @@ -5,6 +5,13 @@ using namespace wolf; using namespace Eigen; +TEST(Polyline2DTest, Transformations) +{ + ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector2s::Zero(),0), 1e-12); + ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector3s::Zero()), 1e-12); + ASSERT_MATRIX_APPROX(Vector3s::Zero(), T2pose2D(Matrix3s::Identity()), 1e-12); +} + TEST(Polyline2DTest, RigidTransformation) { // Random points around random position