diff --git a/CMakeLists.txt b/CMakeLists.txt
index fef41cbba74fb13c7d02841c2ddbb876a0c28959..8d4e1e7e0661b09326ccc803a22d732916081879 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -603,7 +603,6 @@ IF (laser_scan_utils_FOUND)
SET(HDRS_PROCESSOR ${HDRS_PROCESSOR}
include/base/processor/processor_tracker_feature_corner.h
include/base/processor/processor_tracker_landmark_corner.h
- include/base/processor/processor_tracker_landmark_polyline.h
)
SET(HDRS_SENSOR ${HDRS_SENSOR}
include/base/sensor/sensor_laser_2D.h
@@ -612,7 +611,6 @@ IF (laser_scan_utils_FOUND)
src/sensor/sensor_laser_2D.cpp
src/processor/processor_tracker_feature_corner.cpp
src/processor/processor_tracker_landmark_corner.cpp
- src/processor/processor_tracker_landmark_polyline.cpp
)
ENDIF(laser_scan_utils_FOUND)
diff --git a/include/base/processor/processor_tracker_landmark_polyline.h b/include/base/processor/processor_tracker_landmark_polyline.h
deleted file mode 100644
index a6f902c930d696dc595ae19f133a0df4fd0e00c2..0000000000000000000000000000000000000000
--- a/include/base/processor/processor_tracker_landmark_polyline.h
+++ /dev/null
@@ -1,248 +0,0 @@
-/*
- * processor_tracker_landmark_polyline.h
- *
- * Created on: May 26, 2016
- * Author: jvallve
- */
-
-#ifndef SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
-#define SRC_PROCESSOR_TRACKER_LANDMARK_POLYLINE_H_
-
-// Wolf includes
-#include "base/sensor/sensor_laser_2D.h"
-#include "base/capture/capture_laser_2D.h"
-#include "base/feature/feature_polyline_2D.h"
-#include "base/landmark/landmark_polyline_2D.h"
-#include "base/factor/factor_point_2D.h"
-#include "base/factor/factor_point_to_line_2D.h"
-#include "base/state_block.h"
-#include "base/association/association_tree.h"
-#include "base/processor/processor_tracker_landmark.h"
-
-//laser_scan_utils
-#include "laser_scan_utils/laser_scan.h"
-#include "laser_scan_utils/line_finder_iterative.h"
-#include "laser_scan_utils/polyline.h"
-
-namespace wolf
-{
-
-//some consts.. TODO: this tuning params should be grouped in a struct and passed to the class from ros node, at constructor level
-const Scalar position_error_th_ = 1;
-const Scalar min_features_ratio_th_ = 0.5;
-
-//forward declaration to typedef class pointers
-struct LandmarkPolylineMatch;
-typedef std::shared_ptr<LandmarkPolylineMatch> LandmarkPolylineMatchPtr;
-
-//forward declaration to typedef class pointers
-struct ProcessorParamsPolyline;
-typedef std::shared_ptr<ProcessorParamsPolyline> ProcessorParamsPolylinePtr;
-
-WOLF_PTR_TYPEDEFS(ProcessorTrackerLandmarkPolyline);
-
-// Match Feature - Landmark
-struct LandmarkPolylineMatch : public LandmarkMatch
-{
- int landmark_match_from_id_;
- int feature_match_from_id_;
- int landmark_match_to_id_;
- int feature_match_to_id_;
-
-// std::vector<unsigned int> landmark_points_match_;
-// std::vector<unsigned int> feature_points_match_;
-// std::vector<unsigned int> feature_points_add_front_;
-// std::vector<unsigned int> feature_points_add_back_;
-
- LandmarkPolylineMatch() :
- landmark_match_from_id_(0),
- feature_match_from_id_(0),
- landmark_match_to_id_(0),
- feature_match_to_id_(0)
- {
- //
- }
- LandmarkPolylineMatch(int _landmark_match_from_id,
- int _feature_match_from_id,
- int _landmark_match_to_id,
- int _feature_match_to_id) :
- landmark_match_from_id_(_landmark_match_from_id),
- feature_match_from_id_(_feature_match_from_id),
- landmark_match_to_id_(_landmark_match_to_id),
- feature_match_to_id_(_landmark_match_to_id)
- {
- //
- }
-};
-
-WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
-struct ProcessorParamsPolyline : public ProcessorParamsTrackerLandmark
-{
- laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar position_error_th;
- unsigned int new_features_th;
- unsigned int loop_frames_th;
-
- // These values below are constant and defined within the class -- provide a setter or accept them at construction time if you need to configure them
- // Scalar aperture_error_th_ = 20.0 * M_PI / 180.; //20 degrees
- // Scalar angular_error_th_ = 10.0 * M_PI / 180.; //10 degrees;
- // Scalar position_error_th_ = 1;
- // Scalar min_features_ratio_th_ = 0.5;
-};
-
-class ProcessorTrackerLandmarkPolyline : public ProcessorTrackerLandmark
-{
- private:
- laserscanutils::LineFinderIterative line_finder_;
- ProcessorParamsPolylinePtr params_;
-
- FeatureBasePtrList polylines_incoming_;
- FeatureBasePtrList polylines_last_;
-
- Eigen::Matrix2s R_sensor_world_, R_world_sensor_;
- Eigen::Matrix2s R_robot_sensor_;
- Eigen::Matrix2s R_current_prev_;
- Eigen::Vector2s t_sensor_world_, t_world_sensor_, t_world_sensor_prev_, t_sensor_world_prev_;
- Eigen::Vector2s t_robot_sensor_;
- Eigen::Vector2s t_current_prev_;
- Eigen::Vector2s t_world_robot_;
- bool extrinsics_transformation_computed_;
-
- public:
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW; // to guarantee alignment (see http://eigen.tuxfamily.org/dox-devel/group__TopicStructHavingEigenMembers.html)
-
- ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params);
-
- virtual ~ProcessorTrackerLandmarkPolyline();
- virtual void configure(SensorBasePtr _sensor) { };
-
- const FeatureBasePtrList& getLastPolylines() const;
-
- protected:
-
- virtual void preProcess();
- void computeTransformations(const TimeStamp& _ts);
- virtual void postProcess();
-
- void advanceDerived();
-
- void resetDerived();
-
- /** \brief Find provided landmarks in the incoming capture
- * \param _landmarks_in input list of landmarks to be found in incoming
- * \param _features_incoming_out returned list of incoming features corresponding to a landmark of _landmarks_in
- * \param _feature_landmark_correspondences returned map of landmark correspondences: _feature_landmark_correspondences[_feature_out_ptr] = landmark_in_ptr
- */
- virtual unsigned int findLandmarks(const LandmarkBasePtrList& _landmarks_in, FeatureBasePtrList& _features_incoming_out,
- LandmarkMatchMap& _feature_landmark_correspondences);
-
- /** \brief Vote for KeyFrame generation
- *
- * If a KeyFrame criterion is validated, this function returns true,
- * meaning that it wants to create a KeyFrame at the \b last Capture.
- *
- * WARNING! This function only votes! It does not create KeyFrames!
- */
- virtual bool voteForKeyFrame();
-
- /** \brief Detect new Features
- * \param _capture_ptr Capture for feature detection. Defaults to incoming_ptr_.
- * \param _new_features_list The list of detected Features. Defaults to member new_features_list_.
- * \return The number of detected Features.
- *
- * This function detects Features that do not correspond to known Features/Landmarks in the system.
- *
- * The function sets the member new_features_list_, the list of newly detected features,
- * to be used for landmark initialization.
- */
- virtual unsigned int detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out);
-
- /** \brief Creates a landmark for each of new_features_last_
- **/
- virtual void createNewLandmarks();
-
- /** \brief Create one landmark
- *
- * Implement in derived classes to build the type of landmark you need for this tracker.
- */
- virtual LandmarkBasePtr createLandmark(FeatureBasePtr _feature_ptr);
-
- /** \brief Establish factors between features in Captures \b last and \b origin
- */
- virtual void establishFactors();
-
- /** \brief look for known objects in the list of unclassified polylines
- */
- void classifyPolilines(LandmarkBasePtrList& _lmk_list);
-
- /** \brief Create a new factor
- * \param _feature_ptr pointer to the Feature to constrain
- * \param _landmark_ptr LandmarkBase pointer to the Landmark constrained.
- *
- * Implement this method in derived classes.
- */
- virtual FactorBasePtr createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr);
-
- private:
-
- void extractPolylines(CaptureLaser2DPtr _capture_laser_ptr, FeatureBasePtrList& _polyline_list);
-
- void expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
- Eigen::MatrixXs& expected_feature_cov_);
-
- Eigen::VectorXs computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu);
- Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B,
- bool _A_extreme, bool _B_extreme);
- // Factory method
- public:
- static ProcessorBasePtr create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr sensor_ptr = nullptr);
-};
-
-inline ProcessorTrackerLandmarkPolyline::ProcessorTrackerLandmarkPolyline(ProcessorParamsPolylinePtr _params) :
- ProcessorTrackerLandmark("TRACKER LANDMARK POLYLINE", _params),
- line_finder_(_params->line_finder_params),
- params_(_params),
- extrinsics_transformation_computed_(false)
-{
-}
-
-inline unsigned int ProcessorTrackerLandmarkPolyline::detectNewFeatures(const unsigned int& _max_features, FeatureBasePtrList& _features_incoming_out)
-{
- // already computed since each scan is computed in preprocess()
- _features_incoming_out = std::move(polylines_last_);
- return _features_incoming_out.size();
-}
-
-inline void ProcessorTrackerLandmarkPolyline::advanceDerived()
-{
- //std::cout << "\tProcessorTrackerLandmarkPolyline::advance:" << std::endl;
- //std::cout << "\t\tcorners_last: " << polylines_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::advanceDerived();
- for (auto polyline : polylines_last_)
- polyline->remove();
- polylines_last_ = std::move(polylines_incoming_);
- //std::cout << "advanced" << std::endl;
-}
-
-inline void ProcessorTrackerLandmarkPolyline::resetDerived()
-{
- //std::cout << "\tProcessorTrackerLandmarkPolyline::reset:" << std::endl;
- //std::cout << "\t\tcorners_last: " << corners_last_.size() << std::endl;
- //std::cout << "\t\tcorners_incoming_: " << polylines_incoming_.size() << std::endl;
- ProcessorTrackerLandmark::resetDerived();
- polylines_last_ = std::move(polylines_incoming_);
-}
-
-inline const FeatureBasePtrList& ProcessorTrackerLandmarkPolyline::getLastPolylines() const
-{
- return polylines_last_;
-}
-
-} // namespace wolf
-
-#endif /* SRC_PROCESSOR_TRACKER_LASER_H_ */
diff --git a/src/processor/processor_tracker_landmark_polyline.cpp b/src/processor/processor_tracker_landmark_polyline.cpp
deleted file mode 100644
index c3f91c0278eda1a7a73657f4d18d40710c59f999..0000000000000000000000000000000000000000
--- a/src/processor/processor_tracker_landmark_polyline.cpp
+++ /dev/null
@@ -1,1020 +0,0 @@
-#include "base/processor/processor_tracker_landmark_polyline.h"
-
-namespace wolf
-{
-
-void ProcessorTrackerLandmarkPolyline::preProcess()
-{
- //std::cout << "PreProcess: " << std::endl;
-
- // extract corners of incoming
- extractPolylines(std::static_pointer_cast<CaptureLaser2D>(incoming_ptr_), polylines_incoming_);
-
- // compute transformations
- computeTransformations(incoming_ptr_->getTimeStamp());
-
- //std::cout << "PreProcess: incoming new features: " << polylines_incoming_.size() << std::endl;
-}
-
-void ProcessorTrackerLandmarkPolyline::computeTransformations(const TimeStamp& _ts)
-{
- Eigen::Vector3s vehicle_pose = getProblem()->getState(_ts);
- t_world_robot_ = vehicle_pose.head<2>();
-
- // world_robot
- Eigen::Matrix2s R_world_robot = Eigen::Rotation2Ds(vehicle_pose(2)).matrix();
-
- // robot_sensor (to be computed once if extrinsics are fixed and not dynamic)
- if (getSensor()->extrinsicsInCaptures() || !getSensor()->getP()->isFixed()
- || !getSensor()->getO()->isFixed() || !extrinsics_transformation_computed_)
- {
- t_robot_sensor_ = getSensor()->getP()->getState();
- R_robot_sensor_ = Eigen::Rotation2Ds(getSensor()->getO()->getState()(0)).matrix();
- extrinsics_transformation_computed_ = true;
- }
-
- // global_sensor
- R_world_sensor_ = R_world_robot * R_robot_sensor_;
- t_world_sensor_ = t_world_robot_ + R_world_robot * t_robot_sensor_;
-
- // sensor_global
- R_sensor_world_ = R_robot_sensor_.transpose() * R_world_robot.transpose();
- t_sensor_world_ = -R_robot_sensor_.transpose() * t_robot_sensor_ -R_sensor_world_ * t_world_robot_ ;
-
- //std::cout << "t_world_robot_ " << t_world_robot_.transpose() << 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_ " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_world_sensor_ " << std::endl << R_world_sensor_ << std::endl;
- //std::cout << "t_sensor_world_ " << t_sensor_world_.transpose() << std::endl;
- //std::cout << "R_sensor_world_ " << std::endl << R_sensor_world_ << std::endl;
-
-}
-
-unsigned int ProcessorTrackerLandmarkPolyline::findLandmarks(const LandmarkBasePtrList& _landmarks_searched,
- FeatureBasePtrList& _features_found,
- LandmarkMatchMap& _feature_landmark_correspondences)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::findLandmarks: " << _landmarks_searched.size() << " features: " << polylines_incoming_.size() << std::endl;
-
- // COMPUTING ALL EXPECTED FEATURES
- std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features;
- std::map<LandmarkBasePtr, Eigen::MatrixXs> expected_features_covs;
- for (auto landmark : _landmarks_searched)
- if (landmark->getType() == "POLYLINE 2D")
- {
- expected_features[landmark] = Eigen::MatrixXs(3, (std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints());
- expected_features_covs[landmark] = Eigen::MatrixXs(2, 2*(std::static_pointer_cast<LandmarkPolyline2D>(landmark))->getNPoints());
- expectedFeature(landmark, expected_features[landmark], expected_features_covs[landmark]);
- }
-
- // NAIVE NEAREST NEIGHBOR MATCHING
- LandmarkPolylineMatchPtr best_match = nullptr;
- FeaturePolyline2DPtr polyline_feature;
- LandmarkPolyline2DPtr polyline_landmark;
-
- auto next_feature_it = polylines_incoming_.begin();
- auto feature_it = next_feature_it++;
- int max_ftr, max_lmk, max_offset, min_offset, offset, from_ftr, from_lmk, to_ftr, to_lmk, N_overlapped;
-
- // iterate over all polylines features
- while (feature_it != polylines_incoming_.end())
- {
- polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(*feature_it);
- max_ftr = polyline_feature->getNPoints() - 1;
-
- // Check with all landmarks
- for (auto landmark_it = _landmarks_searched.begin(); landmark_it != _landmarks_searched.end(); landmark_it++)
- {
- polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(*landmark_it);
-
- // Open landmark polyline
- if (!polyline_landmark->isClosed())
- {
- //std::cout << "MATCHING WITH OPEN LANDMARK" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << ": 0-" << max_ftr << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl;
- max_lmk = polyline_landmark->getNPoints() - 1;
- max_offset = max_ftr;
- min_offset = -max_lmk;
-
- // Check all overlapping positions between each feature-landmark pair
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- if (offset == min_offset && !polyline_landmark->isLastDefined() && !polyline_feature->isFirstDefined())
- continue;
-
- if (offset == max_offset && !polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined())
- continue;
-
- from_lmk = std::max(0, -offset);
- from_ftr = std::max(0, offset);
- N_overlapped = std::min(max_ftr - from_ftr, max_lmk - from_lmk)+1;
- to_lmk = from_lmk+N_overlapped-1;
- to_ftr = from_ftr+N_overlapped-1;
-
- //std::cout << "\t\toffset " << offset << std::endl;
- //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl;
- //std::cout << "\t\t\tfrom_ftr " << from_ftr << std::endl;
- //std::cout << "\t\t\tN_overlapped " << N_overlapped << std::endl;
-
- // Compute the squared distance for all overlapped points
- Eigen::ArrayXXd d = (polyline_feature->getPoints().block(0,from_ftr, 2,N_overlapped) -
- expected_features[*landmark_it].block(0,from_lmk, 2, N_overlapped)).array();
-
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- if (offset != min_offset && offset != max_offset)
- {
- // Point-to-line first distance
- bool from_ftr_not_defined = (from_ftr == 0 && !polyline_feature->isFirstDefined());
- bool from_lmk_not_defined = (from_lmk == 0 && !polyline_landmark->isFirstDefined());
- //std::cout << "\t\tfrom_ftr_not_defined " << from_ftr_not_defined << (from_ftr == 0) << !polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\t\tfrom_lmk_not_defined " << from_lmk_not_defined << (from_lmk == 0) << !polyline_landmark->isFirstDefined() << std::endl;
- if (from_ftr_not_defined || from_lmk_not_defined)
- {
- //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
- //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(from_lmk).transpose() << std::endl;
- //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(from_lmk+1).transpose() << std::endl;
- //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(from_ftr).transpose() << std::endl;
- dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk),
- expected_features[*landmark_it].col(from_lmk+1),
- polyline_feature->getPoints().col(from_ftr),
- !from_lmk_not_defined,
- !from_ftr_not_defined);
- }
-
- // Point-to-line last distance
- bool last_ftr_not_defined = !polyline_feature->isLastDefined() && to_ftr == max_ftr;
- bool last_lmk_not_defined = !polyline_landmark->isLastDefined() && to_lmk == max_lmk;
- //std::cout << "\t\tlast_ftr_not_defined " << last_ftr_not_defined << (to_ftr == max_ftr) << !polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\t\tlast_lmk_not_defined " << last_lmk_not_defined << (to_lmk == max_lmk) << !polyline_landmark->isLastDefined() << std::endl;
- if (last_ftr_not_defined || last_lmk_not_defined)
- {
- //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
- //std::cout << "\t\t\tA" << expected_features[*landmark_it].col(to_lmk).transpose() << std::endl;
- //std::cout << "\t\t\tAaux" << expected_features[*landmark_it].col(to_lmk-1).transpose() << std::endl;
- //std::cout << "\t\t\tB" << polyline_feature->getPoints().col(to_ftr).transpose() << std::endl;
- dist2(N_overlapped-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk),
- expected_features[*landmark_it].col(to_lmk-1),
- polyline_feature->getPoints().col(to_ftr),
- !last_lmk_not_defined,
- !last_ftr_not_defined);
- }
- }
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be witin a threshold
- // Choose the most overlapped one
- if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr ||
- (N_overlapped >= best_match->feature_match_to_id_-best_match->feature_match_from_id_+1 &&
- dist2.mean() < best_match->normalized_score_ )))
- {
- //std::cout << "BEST MATCH" << std::endl;
- best_match = std::make_shared<LandmarkPolylineMatch>();
- best_match->feature_match_from_id_= from_ftr;
- best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId();
- best_match->feature_match_to_id_= from_ftr+N_overlapped-1;
- best_match->landmark_match_to_id_= from_lmk+N_overlapped-1+polyline_landmark->getFirstId();
- best_match->landmark_ptr_=polyline_landmark;
- best_match->normalized_score_ = dist2.mean();
- }
- }
- }
- // Closed landmark polyline
- else
- {
- if (polyline_feature->getNPoints() > polyline_landmark->getNPoints())
- continue;
-
- //std::cout << "MATCHING WITH CLOSED LANDMARK" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << ": 0-" << max_ftr << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << ": 0-" << polyline_landmark->getNPoints() - 1 << std::endl;
-
- max_offset = 0;
- min_offset = -polyline_landmark->getNPoints() + 1;
-
- // Check all overlapping positions between each feature-landmark pair
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- from_lmk = -offset;
- to_lmk = from_lmk+polyline_feature->getNPoints()-1;
- if (to_lmk >= polyline_landmark->getNPoints())
- to_lmk -= polyline_landmark->getNPoints();
-
- //std::cout << "\t\toffset " << offset << std::endl;
- //std::cout << "\t\t\tfrom_lmk " << from_lmk << std::endl;
- //std::cout << "\t\t\tto_lmk " << to_lmk << std::endl;
-
- // Compute the squared distance for all overlapped points
- Eigen::ArrayXXd d = polyline_feature->getPoints().topRows(2).array();
- if (to_lmk > from_lmk)
- d -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_feature->getNPoints()).array();
- else
- {
- d.leftCols(polyline_landmark->getNPoints()-from_lmk) -= expected_features[*landmark_it].block(0,from_lmk, 2, polyline_landmark->getNPoints()-from_lmk).array();
- d.rightCols(to_lmk+1) -= expected_features[*landmark_it].block(0, 0, 2, to_lmk+1).array();
- }
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // Point-to-line first distance
- if (!polyline_feature->isFirstDefined())
- {
- int next_from_lmk = (from_lmk+1 == polyline_landmark->getNPoints() ? 0 : from_lmk+1);
- dist2(0) = sqDistPointToLine(expected_features[*landmark_it].col(from_lmk),
- expected_features[*landmark_it].col(next_from_lmk),
- polyline_feature->getPoints().col(0),
- true,
- false);
- }
-
- // Point-to-line last distance
- if (!polyline_feature->isLastDefined())
- {
- int prev_to_lmk = (to_lmk == 0 ? polyline_landmark->getNPoints()-1 : to_lmk-1);
- dist2(polyline_feature->getNPoints()-1) = sqDistPointToLine(expected_features[*landmark_it].col(to_lmk),
- expected_features[*landmark_it].col(prev_to_lmk),
- polyline_feature->getPoints().col(polyline_feature->getNPoints()-1),
- true,
- false);
- }
- //std::cout << "\t\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be witin a threshold
- // Choose the most overlapped one
- if ((dist2 < params_->position_error_th*params_->position_error_th).all() && (best_match == nullptr || dist2.mean() < best_match->normalized_score_ ))
- {
- //std::cout << "BEST MATCH" << std::endl;
- best_match = std::make_shared<LandmarkPolylineMatch>();
- best_match->feature_match_from_id_= 0;
- best_match->landmark_match_from_id_= from_lmk+polyline_landmark->getFirstId();
- best_match->feature_match_to_id_= polyline_feature->getNPoints()-1;
- best_match->landmark_match_to_id_= to_lmk+polyline_landmark->getFirstId();
- best_match->landmark_ptr_=polyline_landmark;
- best_match->normalized_score_ = dist2.mean();
- }
- }
- }
-
- //std::cout << "landmark " << (*landmark_it)->id() << ": 0-" << max_lmk << " - defined " << polyline_landmark->isFirstDefined() << polyline_landmark->isLastDefined() << std::endl;
- //std::cout << "feature " << (*feature_it)->id() << ": 0-" << max_ftr << " - defined " << polyline_feature->isFirstDefined() << polyline_feature->isLastDefined() << std::endl;
- //std::cout << expected_features[*landmark_it] << std::endl;
- //std::cout << "\tmax_offset " << max_offset << std::endl;
- //std::cout << "\tmin_offset " << min_offset << std::endl;
- //if (!polyline_landmark->isFirstDefined() && !polyline_feature->isLastDefined())
- // std::cout << "\tLIMITED max offset " << max_offset << std::endl;
- //if (!polyline_feature->isFirstDefined() && !polyline_landmark->isLastDefined())
- // std::cout << "\tLIMITED min offset " << min_offset << std::endl;
-
- }
- // Match found for this feature
- if (best_match != nullptr)
- {
- //std::cout << "\tclosest landmark: " << best_match->landmark_ptr_->id() << std::endl;
- // match
- matches_landmark_from_incoming_[*feature_it] = best_match;
- // move corner feature to output list
- _features_found.splice(_features_found.end(), polylines_incoming_, feature_it);
- // reset match
- best_match = nullptr;
- }
- //else
- //{
- // std::cout << "\t-------------------------->NO LANDMARK CLOSE ENOUGH!!!!" << std::endl;
- // std::getchar();
- //}
- feature_it = next_feature_it++;
- }
- return matches_landmark_from_incoming_.size();
-}
-
-bool ProcessorTrackerLandmarkPolyline::voteForKeyFrame()
-{
- //std::cout << "------------- ProcessorTrackerLandmarkPolyline::voteForKeyFrame:" << std::endl;
- //std::cout << "polylines_last_.size():" << polylines_last_.size()<< std::endl;
- // option 1: more than TH new features in last
- if (polylines_last_.size() >= params_->new_features_th)
- {
- std::cout << "------------- NEW KEY FRAME: Option 1 - Enough new features" << std::endl;
- //std::cout << "\tnew features in last = " << corners_last_.size() << std::endl;
- return true;
- }
- // option 2: loop closure (if the newest frame from which a matched landmark was observed is old enough)
- for (auto new_ftr : new_features_last_)
- {
- if (last_ptr_->getFrame()->id() - matches_landmark_from_last_[new_ftr]->landmark_ptr_->getConstrainedByList().back()->getCapture()->getFrame()->id() > params_->loop_frames_th)
- {
- std::cout << "------------- NEW KEY FRAME: Option 2 - Loop closure" << std::endl;
- return true;
- }
- }
- return false;
-}
-
-void ProcessorTrackerLandmarkPolyline::extractPolylines(CaptureLaser2DPtr _capture_laser_ptr,
- FeatureBasePtrList& _polyline_list)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::extractPolylines: " << std::endl;
- // TODO: sort corners by bearing
- std::list<laserscanutils::Polyline> polylines;
-
- line_finder_.findPolylines(_capture_laser_ptr->getScan(), (std::static_pointer_cast<SensorLaser2D>(getSensor()))->getScanParams(), polylines);
-
- for (auto&& pl : polylines)
- {
- //std::cout << "new polyline detected: Defined" << pl.first_defined_ << pl.last_defined_ << std::endl;
- //std::cout << "covs: " << std::endl << pl.covs_ << std::endl;
- _polyline_list.push_back(std::make_shared<FeaturePolyline2D>(pl.points_, pl.covs_, pl.first_defined_, pl.last_defined_));
- //std::cout << "new polyline detected: " << std::endl;
- }
- //std::cout << _polyline_list.size() << " polylines extracted" << std::endl;
-}
-
-void ProcessorTrackerLandmarkPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_,
- Eigen::MatrixXs& expected_feature_cov_)
-{
- assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerLandmarkPolyline::expectedFeature: Bad landmark type");
- LandmarkPolyline2DPtr polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
- assert(expected_feature_.cols() == polyline_landmark->getNPoints() && expected_feature_.rows() == 3 && "ProcessorTrackerLandmarkPolyline::expectedFeature: bad expected_feature_ sizes");
-
- //std::cout << "ProcessorTrackerLandmarkPolyline::expectedFeature" << std::endl;
- //std::cout << "t_world_sensor_: " << t_world_sensor_.transpose() << std::endl;
- //std::cout << "R_sensor_world_: " << std::endl << R_sensor_world_ << std::endl;
-
- expected_feature_ = Eigen::MatrixXs::Zero(3,polyline_landmark->getNPoints());
- expected_feature_cov_ = Eigen::MatrixXs::Zero(2,2*polyline_landmark->getNPoints());
- Eigen::Vector3s col = Eigen::Vector3s::Ones();
-
- ////////// global coordinates points
- if (polyline_landmark->getClassification() == UNCLASSIFIED)
- for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
- {
- //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl;
- //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl;
-
- // ------------ expected feature point
- col.head<2>() = R_sensor_world_ * (polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) - t_world_sensor_);
- expected_feature_.col(i) = col;
-
- //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl;
- // ------------ expected feature point covariance
- // TODO
- expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
- }
-
- ////////// landmark with origin
- else
- {
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(polyline_landmark->getO()->getState()(0)).matrix();
- const Eigen::VectorXs& t_world_points = polyline_landmark->getP()->getState();
-
- for (auto i = 0; i < polyline_landmark->getNPoints(); i++)
- {
- //std::cout << "Point " << i+polyline_landmark->getFirstId() << std::endl;
- //std::cout << "First Point " << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "Landmark global position: " << polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()).transpose() << std::endl;
-
- // ------------ expected feature point
- col.head<2>() = R_sensor_world_ * (R_world_points * polyline_landmark->getPointVector(i+polyline_landmark->getFirstId()) + t_world_points - t_world_sensor_);
- expected_feature_.col(i) = col;
-
- //std::cout << "Expected point " << i << ": " << expected_feature_.col(i).transpose() << std::endl;
- // ------------ expected feature point covariance
- // TODO
- expected_feature_cov_.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
- }
- }
-}
-
-Eigen::VectorXs ProcessorTrackerLandmarkPolyline::computeSquaredMahalanobisDistances(const Eigen::Vector2s& _feature,
- const Eigen::Matrix2s& _feature_cov,
- const Eigen::Vector2s& _expected_feature,
- const Eigen::Matrix2s& _expected_feature_cov,
- const Eigen::MatrixXs& _mu)
-{
- // ------------------------ d
- Eigen::Vector2s d = _feature - _expected_feature;
-
- // ------------------------ Sigma_d
- Eigen::Matrix2s iSigma_d = (_feature_cov + _expected_feature_cov).inverse();
- Eigen::VectorXs squared_mahalanobis_distances(_mu.cols());
- for (unsigned int i = 0; i < _mu.cols(); i++)
- {
- squared_mahalanobis_distances(i) = (d - _mu.col(i)).transpose() * iSigma_d * (d - _mu.col(i));
- //if ((d - _mu.col(i)).norm() < 1)
- //{
- // std::cout << "distance: " << (d - _mu.col(i)).norm() << std::endl;
- // std::cout << "iSigma_d: " << std::endl << iSigma_d << std::endl;
- // std::cout << "mahalanobis: " << squared_mahalanobis_distances(i) << std::endl;
- //}
- }
-
- return squared_mahalanobis_distances;
-}
-
-Scalar ProcessorTrackerLandmarkPolyline::sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined)
-{
- /* Squared distance from B to the line A_aux-A (match evaluated is A-B)
- *
- * No matter if A_aux is defined or not.
- *
- * Case 1: B not defined
- *
- * The projection of B over the line AAaux must be in [A_aux, inf(in A direction)). Otherwise, return squared distance Aaux-B.
- * Check: the angle BAauxA is <= 90º:
- * (BA)² <= (BAaux)² + (AAaux)²
- *
- * Case 1.1: A not defined
- *
- * No more restrictions: return distance line to point.
- *
- * Case 1.2: A defined
- *
- * The projection of B over the line AAaux must be in (inf(in Aaux direction), A]. Otherwise, return squared distance A-B.
- * Additional check: the angle BAAaux is <= 90º:
- * (BAaux)² <= (BA)² + (AAaux)²
- *
- * Case 2: B is defined and A is not
- *
- * Returns the distance B-Line if the projection of B to the line is in [A, inf). Otherwise, return squared distance A-B.
- * Checks if the angle BAAaux is >= 90º: (BAaux)² >= (BA)² + (AAaux)²
- *
- * ( Case B and A are defined is not point-to-line, is point to point -> assertion )
- *
- */
-
- assert((!_A_defined || !_B_defined) && "ProcessorTrackerLandmarkPolyline::sqDistPointToLine: at least one point must not be defined.");
-
- Scalar AB_sq = (_B-_A).head<2>().squaredNorm();
- Scalar AauxB_sq = (_B-_A_aux).head<2>().squaredNorm();
- Scalar AAaux_sq = (_A_aux-_A).head<2>().squaredNorm();
-
- // Case 1
- if (!_B_defined)
- {
- if (AB_sq <= AauxB_sq + AAaux_sq)
- {
- // Case 1.1
- if (!_A_defined)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
- // Case 1.2
- else if (AauxB_sq <= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
- }
- }
- // Case 2
- else if (!_A_defined && _B_defined)
- if (AauxB_sq >= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq; //squared distance to line
-
- // Default return A-B squared distance
- return (_A.head<2>() - _B.head<2>()).squaredNorm();
-}
-
-void ProcessorTrackerLandmarkPolyline::createNewLandmarks()
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::createNewLandmarks" << std::endl;
- FeaturePolyline2DPtr polyline_ft_ptr;
- LandmarkBasePtr new_lmk_ptr;
- for (auto new_feature_ptr : new_features_last_)
- {
- // create new landmark
- new_lmk_ptr = createLandmark(new_feature_ptr);
- //std::cout << "\tfeature: " << new_feature_ptr->id() << std::endl;
- //std::cout << "\tnew_landmark: " << new_lmk_ptr->id() << ": "<< ((LandmarkPolyline2D*)new_lmk_ptr)->getNPoints() << " points" << std::endl;
- new_landmarks_.push_back(new_lmk_ptr);
- // cast
- polyline_ft_ptr = std::static_pointer_cast<FeaturePolyline2D>(new_feature_ptr);
- // create new correspondence
- LandmarkPolylineMatchPtr match = std::make_shared<LandmarkPolylineMatch>();
- match->feature_match_from_id_= 0; // all points match
- match->landmark_match_from_id_ = 0;
- match->feature_match_to_id_= polyline_ft_ptr->getNPoints()-1; // all points match
- match->landmark_match_to_id_ = polyline_ft_ptr->getNPoints()-1;
- match->normalized_score_ = 1.0; // max score
- match->landmark_ptr_ = new_lmk_ptr;
- matches_landmark_from_last_[new_feature_ptr] = match;
- }
-}
-
-LandmarkBasePtr ProcessorTrackerLandmarkPolyline::createLandmark(FeatureBasePtr _feature_ptr)
-{
- assert(_feature_ptr->getType() == "POLYLINE 2D");
- //std::cout << "ProcessorTrackerLandmarkPolyline::createLandmark" << std::endl;
- //std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
- //std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
-
- FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
- // compute feature global pose
- Eigen::MatrixXs points_global = R_world_sensor_ * polyline_ptr->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(polyline_ptr->getNPoints()).transpose();
-
- //std::cout << "Feature local points: " << std::endl << polyline_ptr->getPoints().topRows<2>() << std::endl;
- //std::cout << "Landmark global points: " << std::endl << points_global << std::endl;
- //std::cout << "New landmark: extremes defined " << polyline_ptr->isFirstDefined() << polyline_ptr->isLastDefined() << std::endl;
-
- // Create new landmark
- return std::make_shared<LandmarkPolyline2D>(std::make_shared<StateBlock>(Eigen::Vector2s::Zero(), true), std::make_shared<StateBlock>(Eigen::Vector1s::Zero(), true), points_global, polyline_ptr->isFirstDefined(), polyline_ptr->isLastDefined());
-}
-
-ProcessorTrackerLandmarkPolyline::~ProcessorTrackerLandmarkPolyline()
-{
- while (!polylines_last_.empty())
- {
- polylines_last_.front()->remove();
- polylines_last_.pop_front(); // TODO see if this is needed
- }
- while (!polylines_incoming_.empty())
- {
- polylines_incoming_.front()->remove();
- polylines_incoming_.pop_front(); // TODO see if this is needed
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::establishFactors()
-{
- //TODO: update with new index in landmarks
-
- //std::cout << "ProcessorTrackerLandmarkPolyline::establishFactors" << std::endl;
- LandmarkPolylineMatchPtr polyline_match;
- FeaturePolyline2DPtr polyline_feature;
- LandmarkPolyline2DPtr polyline_landmark;
-
- for (auto last_feature : last_ptr_->getFeatureList())
- {
- polyline_feature = std::static_pointer_cast<FeaturePolyline2D>(last_feature);
- polyline_match = std::static_pointer_cast<LandmarkPolylineMatch>(matches_landmark_from_last_[last_feature]);
- polyline_landmark = std::static_pointer_cast<LandmarkPolyline2D>(polyline_match->landmark_ptr_);
-
- assert(polyline_landmark != nullptr && polyline_match != nullptr);
-
- // Modify landmark (only for not closed)
- if (!polyline_landmark->isClosed())
- {
- //std::cout << std::endl << "MODIFY LANDMARK" << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- Eigen::MatrixXs points_global = R_world_sensor_ * polyline_feature->getPoints().topRows<2>() +
- t_world_sensor_ * Eigen::VectorXs::Ones(polyline_feature->getNPoints()).transpose();
- // GROW/CLOSE LANDMARK
- // -----------------Front-----------------
- bool check_front_closing = // Sufficient conditions
- // condition 1: feature first defined point not matched
- (polyline_feature->isFirstDefined() && polyline_match->feature_match_from_id_ > 0) ||
- // condition 2: feature second point not matched
- (polyline_match->feature_match_from_id_ > 1) ||
- // condition 3: matched front points but feature front point defined and landmark front point not defined
- (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && polyline_feature->isFirstDefined() && !polyline_landmark->isFirstDefined());
-
- // Check closing with landmark's last points
- if (check_front_closing)
- {
- //std::cout << "---------------- Trying to close polyline..." << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- int feat_point_id_matching = polyline_feature->isFirstDefined() ? 0 : 1;
- int lmk_last_defined_point = polyline_landmark->getLastId() - (polyline_landmark->isLastDefined() ? 0 : 1);
- //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl;
- //std::cout << std::endl << "\tlmk last defined point " << lmk_last_defined_point << std::endl;
-
- for (int id_lmk = lmk_last_defined_point; id_lmk > polyline_match->landmark_match_to_id_; id_lmk--)
- {
- //std::cout << "\t\tid_lmk " << id_lmk << std::endl;
- //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
-
- if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
- {
- std::cout << "CLOSING POLYLINE" << std::endl;
-
- unsigned int N_back_overlapped = polyline_landmark->getLastId() - id_lmk + 1;
- int N_feature_new_points = polyline_match->feature_match_from_id_ - feat_point_id_matching - N_back_overlapped;
-
- // define first point (if not defined and no points have to be merged)
- if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0)
- polyline_landmark->setFirst(points_global.col(polyline_match->feature_match_from_id_), true);
-
- // add other points (if there are)
- if (N_feature_new_points > 0)
- polyline_landmark->addPoints(points_global.middleCols(feat_point_id_matching + N_back_overlapped, N_feature_new_points), // points matrix in global coordinates
- N_feature_new_points-1, // last index to be added
- true, // defined
- false); // front (!back)
-
- // define last point (if not defined and no points have to be merged)
- if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0)
- polyline_landmark->setLast(points_global.col(feat_point_id_matching + N_back_overlapped - 1), true);
-
- // close landmark
- polyline_landmark->setClosed();
-
- polyline_match->landmark_match_from_id_ = id_lmk - (polyline_feature->isFirstDefined() ? 0 : 1);
- polyline_match->feature_match_from_id_ = 0;
- break;
- }
- }
- }
- // Add new front points (if not matched feature points)
- if (polyline_match->feature_match_from_id_ > 0) // && !polyline_landmark->isClosed()
- {
- assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark");
- //std::cout << "Add new front points. Defined: " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-
- polyline_landmark->addPoints(points_global, // points matrix in global coordinates
- polyline_match->feature_match_from_id_-1, // last feature point index to be added
- polyline_feature->isFirstDefined(), // is defined
- false); // front
-
- polyline_match->landmark_match_from_id_ = polyline_landmark->getFirstId();
- polyline_match->feature_match_from_id_ = 0;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
- }
- // Change first point
- else if (polyline_match->landmark_match_from_id_ == polyline_landmark->getFirstId() && !polyline_landmark->isFirstDefined())
- {
- //std::cout << "Change first point. Defined: " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getFirstId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()).transpose() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getFirstId()+1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1).transpose() << std::endl;
- //std::cout << "\tfeature point: " << points_global.topLeftCorner(2,1).transpose() << std::endl;
-
- if (// new defined first
- polyline_feature->isFirstDefined() ||
- // new not defined first
- (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)).squaredNorm() >
- (points_global.topLeftCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm() +
- (polyline_landmark->getPointVector(polyline_landmark->getFirstId()+1)-polyline_landmark->getPointVector(polyline_landmark->getFirstId())).squaredNorm())
- polyline_landmark->setFirst(points_global.leftCols(1), polyline_feature->isFirstDefined());
-
- }
- // -----------------Back-----------------
- bool check_back_closing = // Sufficient conditions
- // condition 1: feature last defined point not matched
- (polyline_feature->isLastDefined() && polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1) ||
- // condition 2: feature second last point not matched
- (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints() - 2) ||
- // condition 3: matched back points but feature last point defined and landmark last point not defined
- (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && polyline_feature->isLastDefined() && !polyline_landmark->isLastDefined());
-
- // Necessary condition: still open landmark
- check_back_closing = check_back_closing && !polyline_landmark->isClosed();
-
- // Check closing with landmark's first points
- if (check_back_closing)
- {
- int feat_point_id_matching = polyline_feature->getNPoints() - (polyline_feature->isLastDefined() ? 1 : 2);
- int lmk_first_defined_point = polyline_landmark->getFirstId() + (polyline_landmark->isFirstDefined() ? 0 : 1);
- //std::cout << std::endl << "\tfeat point matching " << feat_point_id_matching << std::endl;
- //std::cout << std::endl << "\tlmk first defined point " << lmk_first_defined_point << std::endl;
-
- for (int id_lmk = lmk_first_defined_point; id_lmk < polyline_match->landmark_match_from_id_; id_lmk++)
- {
- //std::cout << "\t\tid_lmk " << id_lmk << std::endl;
- //std::cout << "\t\td2 = " << (points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() << " (th = " << params_->position_error_th*params_->position_error_th << std::endl;
-
- if ((points_global.col(feat_point_id_matching)-polyline_landmark->getPointVector(id_lmk)).squaredNorm() < params_->position_error_th*params_->position_error_th)
- {
- std::cout << "CLOSING POLYLINE" << std::endl;
-
- unsigned int N_front_overlapped = id_lmk - polyline_landmark->getFirstId() + 1;
- int N_feature_new_points = feat_point_id_matching - polyline_match->feature_match_to_id_ - N_front_overlapped;
-
- // define first point (if not defined and no points have to be merged)
- if (!polyline_landmark->isFirstDefined() && N_feature_new_points >= 0)
- polyline_landmark->setFirst(points_global.col(feat_point_id_matching - N_front_overlapped + 1), true);
-
- // add other points (if there are)
- if (N_feature_new_points > 0)
- polyline_landmark->addPoints(points_global.middleCols(polyline_match->feature_match_to_id_ + 1, N_feature_new_points), // points matrix in global coordinates
- N_feature_new_points-1, // last index to be added
- true, // defined
- false); // front (!back)
-
- // define last point (if not defined and no points have to be merged)
- if (!polyline_landmark->isLastDefined() && N_feature_new_points >= 0)
- polyline_landmark->setLast(points_global.col(polyline_match->feature_match_to_id_), true);
-
- // close landmark
- polyline_landmark->setClosed();
-
- polyline_match->landmark_match_to_id_ = id_lmk + (polyline_feature->isLastDefined() ? 0 : 1);
- polyline_match->feature_match_to_id_ = polyline_feature->getNPoints() - 1;
- break;
- }
- }
- }
- // Add new back points (if it wasn't closed)
- if (polyline_match->feature_match_to_id_ < polyline_feature->getNPoints()-1)
- {
- assert(!polyline_landmark->isClosed() && "feature not matched points in a closed landmark");
- //std::cout << "Add back points. Defined: " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
-
- polyline_landmark->addPoints(points_global, // points matrix in global coordinates
- polyline_match->feature_match_to_id_+1, // last feature point index to be added
- polyline_feature->isLastDefined(), // is defined
- true); // back
-
- polyline_match->landmark_match_to_id_ = polyline_landmark->getLastId();
- polyline_match->feature_match_to_id_ = polyline_feature->getNPoints()-1;
- //std::cout << "\tfeat from " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << polyline_match->landmark_match_to_id_ << std::endl;
- }
- // Change last point
- else if (polyline_match->landmark_match_to_id_ == polyline_landmark->getLastId() && !polyline_landmark->isLastDefined()) //&& polyline_match->feature_match_to_id_ == polyline_feature->getNPoints()-1
- {
- //std::cout << "Change last point. Defined: " << (polyline_feature->isLastDefined() ? 1 : 0) << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getLastId() << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()).transpose() << std::endl;
- //std::cout << "\tpoint " << polyline_landmark->getLastId()-1 << ": " << polyline_landmark->getPointVector(polyline_landmark->getLastId()-1).transpose() << std::endl;
- //std::cout << "\tfeature point: " << points_global.topRightCorner(2,1).transpose() << std::endl;
- if (// new defined last
- polyline_feature->isLastDefined() ||
- // new not defined last
- (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)).squaredNorm() >
- (points_global.topRightCorner(2,1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm() +
- (polyline_landmark->getPointVector(polyline_landmark->getLastId()-1)-polyline_landmark->getPointVector(polyline_landmark->getLastId())).squaredNorm())
- polyline_landmark->setLast(points_global.rightCols(1), polyline_feature->isLastDefined());
- }
-
- //std::cout << "MODIFIED LANDMARK" << std::endl;
- //std::cout << "feature " << polyline_feature->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_feature->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_feature->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_feature->isLastDefined() << std::endl;
- //std::cout << "landmark " << polyline_landmark->id() << ": " << std::endl;
- //std::cout << "\tpoints " << polyline_landmark->getNPoints() << std::endl;
- //std::cout << "\tfirst defined " << polyline_landmark->isFirstDefined() << std::endl;
- //std::cout << "\tlast defined " << polyline_landmark->isLastDefined() << std::endl << std::endl;
- }
-
- // ESTABLISH CONSTRAINTS
- //std::cout << "ESTABLISH CONSTRAINTS" << std::endl;
- //std::cout << "\tfeature " << polyline_feature->id() << std::endl;
- //std::cout << "\tlandmark " << polyline_landmark->id() << std::endl;
- //std::cout << "\tmatch from feature point " << polyline_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << polyline_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << polyline_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << polyline_match->landmark_match_to_id_ << std::endl;
-
- // Factors for all inner and defined feature points
- int lmk_point_id = polyline_match->landmark_match_from_id_;
-
- for (int ftr_point_id = 0; ftr_point_id < polyline_feature->getNPoints(); ftr_point_id++, lmk_point_id++)
- {
- if (lmk_point_id > polyline_landmark->getLastId())
- lmk_point_id -= polyline_landmark->getNPoints();
- if (lmk_point_id < polyline_landmark->getFirstId())
- lmk_point_id += polyline_landmark->getNPoints();
-
- //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 && !polyline_feature->isFirstDefined())
- // first point to line factor
- {
- int lmk_next_point_id = lmk_point_id+1;
- if (lmk_next_point_id > polyline_landmark->getLastId())
- lmk_next_point_id -= polyline_landmark->getNPoints();
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_next_point_id << std::endl;
- last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id, lmk_next_point_id));
- //std::cout << "factor added" << std::endl;
- }
-
- // Last not defined point
- else if (ftr_point_id == polyline_feature->getNPoints()-1 && !polyline_feature->isLastDefined())
- // last point to line factor
- {
- int lmk_prev_point_id = lmk_point_id-1;
- if (lmk_prev_point_id < polyline_landmark->getFirstId())
- lmk_prev_point_id += polyline_landmark->getNPoints();
- //std::cout << "point-line: landmark points " << lmk_point_id << ", " << lmk_prev_point_id << std::endl;
- last_feature->addFactor(std::make_shared<FactorPointToLine2D>(polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id, lmk_prev_point_id));
- //std::cout << "factor added" << std::endl;
- }
-
- // Defined point
- else
- // point to point factor
- {
- //std::cout << "point-point: landmark point " << lmk_point_id << std::endl;
- //std::cout << "landmark first id:" << polyline_landmark->getFirstId() << std::endl;
- //std::cout << "landmark last id:" << polyline_landmark->getLastId() << std::endl;
- //std::cout << "landmark n points:" << polyline_landmark->getNPoints() << std::endl;
- last_feature->addFactor(std::make_shared<FactorPoint2D>(polyline_feature, polyline_landmark, shared_from_this(),
- ftr_point_id, lmk_point_id));
- //std::cout << "factor added" << std::endl;
- }
- }
- //std::cout << "Factors established" << std::endl;
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::classifyPolilines(LandmarkBasePtrList& _lmk_list)
-{
- //std::cout << "ProcessorTrackerLandmarkPolyline::classifyPolilines: " << _lmk_list->size() << std::endl;
- std::vector<Scalar> object_L({12, 5.9, 1.2});
- std::vector<Scalar> object_W({2.345, 2.345, 0.9});
- std::vector<Scalar> object_D({sqrt(12*12+2.345*2.345), sqrt(5.9*5.9+2.345*2.345), sqrt(0.9*0.9+1.2*1.2)});
- std::vector<LandmarkClassification> object_class({CONTAINER, SMALL_CONTAINER, PALLET});
-
- for (auto lmk_ptr : _lmk_list)
- if (lmk_ptr->getType() == "POLYLINE 2D")
- {
- LandmarkPolyline2DPtr polyline_ptr = std::static_pointer_cast<LandmarkPolyline2D>(lmk_ptr);
- auto n_defined_points = polyline_ptr->getNPoints() - (polyline_ptr->isFirstDefined() ? 0 : 1) - (polyline_ptr->isLastDefined() ? 0 : 1);
- auto A_id = polyline_ptr->getFirstId() + (polyline_ptr->isFirstDefined() ? 0 : 1);
- auto B_id = A_id + 1;
- auto C_id = B_id + 1;
- auto D_id = C_id + 1;
-
- // necessary conditions
- if (polyline_ptr->getClassification() != UNCLASSIFIED ||
- n_defined_points < 3 ||
- n_defined_points > 4 )
- continue;
-
- //std::cout << "landmark " << lmk_ptr->id() << std::endl;
-
- // consider 3 first defined points
- Scalar dAB = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(B_id)).norm();
- Scalar dBC = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(C_id)).norm();
- Scalar dAC = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(C_id)).norm();
-
- //std::cout << "dAB = " << dAB << " error 1: " << fabs(dAB-CONT_L) << " error 2: " << fabs(dAB-CONT_W) << std::endl;
- //std::cout << "dBC = " << dBC << " error 1: " << fabs(dBC-CONT_W) << " error 2: " << fabs(dBC-CONT_L) << std::endl;
- //std::cout << "dAC = " << dAC << " error 1&2: " << fabs(dAC-CONT_D) << std::endl;
-
- auto classification = -1;
- bool configuration;
-
- for (unsigned int i = 0; i < object_L.size(); i++)
- {
- // check configuration 1
- if(fabs(dAB-object_L[i]) < params_->position_error_th &&
- fabs(dBC-object_W[i]) < params_->position_error_th &&
- fabs(dAC-object_D[i]) < params_->position_error_th)
- {
- configuration = true;
- classification = i;
- break;
- }
-
- // check configuration 2
- if(fabs(dAB-object_W[i]) < params_->position_error_th &&
- fabs(dBC-object_L[i]) < params_->position_error_th &&
- fabs(dAC-object_D[i]) < params_->position_error_th)
- {
- configuration = false;
- classification = i;
- break;
- }
- }
-
- // any container position fitted
- if (classification != -1)
- {
- // if 4 defined checking
- if (n_defined_points == 4)
- {
- //std::cout << "checking with 4th point... " << std::endl;
-
- Scalar dAD = (polyline_ptr->getPointVector(A_id) - polyline_ptr->getPointVector(D_id)).norm();
- Scalar dBD = (polyline_ptr->getPointVector(B_id) - polyline_ptr->getPointVector(D_id)).norm();
- Scalar dCD = (polyline_ptr->getPointVector(C_id) - polyline_ptr->getPointVector(D_id)).norm();
-
- // necessary conditions
- if (fabs(dAD-dBC) > params_->position_error_th ||
- fabs(dBD-dAC) > params_->position_error_th ||
- fabs(dCD-dAB) > params_->position_error_th)
- continue;
- }
-
- // if not 4 defined add/define points
- else
- {
- //std::cout << "adding/defining points... " << std::endl;
- if (!polyline_ptr->isFirstDefined())
- {
- polyline_ptr->defineExtreme(false);
- D_id = polyline_ptr->getFirstId();
- }
- else if (!polyline_ptr->isLastDefined())
- polyline_ptr->defineExtreme(true);
- else
- polyline_ptr->addPoint(Eigen::Vector2s::Zero(), true, true);
- }
- //std::cout << "landmark " << lmk_ptr->id() << " classified as " << object_class[classification] << " in configuration " << configuration << std::endl;
-
- // Close
- polyline_ptr->setClosed();
-
- // Classify
- polyline_ptr->classify(object_class[classification]);
-
- // Unfix origin
- polyline_ptr->getP()->unfix();
- polyline_ptr->getO()->unfix();
-
- // Move origin to B
- polyline_ptr->getP()->setState(polyline_ptr->getPointVector((configuration ? B_id : A_id)));
- Eigen::Vector2s frame_x = (configuration ? polyline_ptr->getPointVector(A_id)-polyline_ptr->getPointVector(B_id) : polyline_ptr->getPointVector(C_id)-polyline_ptr->getPointVector(B_id));
- polyline_ptr->getO()->setState(Eigen::Vector1s::Constant(atan2(frame_x(1),frame_x(0))));
-
- //std::cout << "A: " << polyline_ptr->getPointVector(A_id).transpose() << std::endl;
- //std::cout << "B: " << polyline_ptr->getPointVector(B_id).transpose() << std::endl;
- //std::cout << "C: " << polyline_ptr->getPointVector(C_id).transpose() << std::endl;
- //std::cout << "frame_x: " << frame_x.transpose() << std::endl;
- //std::cout << "frame position: " << polyline_ptr->getP()->getVector().transpose() << std::endl;
- //std::cout << "frame orientation: " << polyline_ptr->getO()->getVector() << std::endl;
-
- // Fix polyline points to its respective relative positions
- if (configuration)
- {
- polyline_ptr->getPointStateBlock(A_id)->setState(Eigen::Vector2s(object_L[classification], 0));
- polyline_ptr->getPointStateBlock(B_id)->setState(Eigen::Vector2s(0, 0));
- polyline_ptr->getPointStateBlock(C_id)->setState(Eigen::Vector2s(0, object_W[classification]));
- polyline_ptr->getPointStateBlock(D_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
- }
- else
- {
- polyline_ptr->getPointStateBlock(A_id)->setState(Eigen::Vector2s(0, 0));
- polyline_ptr->getPointStateBlock(B_id)->setState(Eigen::Vector2s(0, object_W[classification]));
- polyline_ptr->getPointStateBlock(C_id)->setState(Eigen::Vector2s(object_L[classification], object_W[classification]));
- polyline_ptr->getPointStateBlock(D_id)->setState(Eigen::Vector2s(object_L[classification], 0));
- }
- for (auto id = polyline_ptr->getFirstId(); id <= polyline_ptr->getLastId(); id++)
- {
- polyline_ptr->getPointStateBlock(id)->fix();
- }
- }
- }
-}
-
-void ProcessorTrackerLandmarkPolyline::postProcess()
-{
- //std::cout << "postProcess: " << std::endl;
- //std::cout << "New Last features: " << getNewFeaturesListLast().size() << std::endl;
- //std::cout << "Last features: " << last_ptr_->getFeatureList().size() << std::endl;
- classifyPolilines(getProblem()->getMap()->getLandmarkList());
-}
-
-FactorBasePtr ProcessorTrackerLandmarkPolyline::createFactor(FeatureBasePtr _feature_ptr, LandmarkBasePtr _landmark_ptr)
-{
- // not used
- return nullptr;
-}
-
-ProcessorBasePtr ProcessorTrackerLandmarkPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
-{
- ProcessorParamsPolylinePtr params = std::static_pointer_cast<ProcessorParamsPolyline>(_params);
- ProcessorTrackerLandmarkPolylinePtr prc_ptr = std::make_shared<ProcessorTrackerLandmarkPolyline>(params);
- prc_ptr->setName(_unique_name);
- return prc_ptr;
-}
-
-} //namespace wolf
-
-// Register in the SensorFactory
-#include "base/processor/processor_factory.h"
-namespace wolf {
-WOLF_REGISTER_PROCESSOR("POLYLINE", ProcessorTrackerLandmarkPolyline)
-} // namespace wolf