diff --git a/include/laser/processor/processor_tracker_feature_polyline_2D.h b/include/laser/processor/processor_tracker_feature_polyline_2D.h
index 918a179610f325ddcf0a21fd69ec3663087ffacf..9d4b448a422d294a5132bba917bd0abbbd2caec3 100644
--- a/include/laser/processor/processor_tracker_feature_polyline_2D.h
+++ b/include/laser/processor/processor_tracker_feature_polyline_2D.h
@@ -37,9 +37,11 @@ typedef std::map<Scalar,LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DScala
struct ProcessorParamsTrackerFeaturePolyline2D : public ProcessorParamsTrackerFeature
{
laserscanutils::LineFinderIterativeParams line_finder_params;
- Scalar match_alignment_position_sq_norm_th;
- Scalar match_landmark_pose_sq_norm_th;
- Scalar match_feature_pose_sq_norm_th;
+ Scalar match_feature_position_sq_norm_th;
+ Scalar match_feature_orientation_sq_norm_th;
+ Scalar match_landmark_position_sq_norm_th;
+ Scalar match_landmark_orientation_sq_norm_th;
+ bool use_probabilistic_match;
Scalar class_position_error_th;
unsigned int new_features_th;
Scalar loop_time_th;
diff --git a/src/processor/processor_tracker_feature_polyline_2D.cpp b/src/processor/processor_tracker_feature_polyline_2D.cpp
index b28e0e43eb0961d62b1c4d2bf6a8dc3df3ee7e12..d2d71a9a870dfaa9de673b941f25de51aa8ca290 100644
--- a/src/processor/processor_tracker_feature_polyline_2D.cpp
+++ b/src/processor/processor_tracker_feature_polyline_2D.cpp
@@ -833,17 +833,60 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithFeature(FeatureMatchPolyline
{
//WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithFeature: ");
- // compute best sq distance matching
- Eigen::MatrixXs last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
- MatchPolyline2DPtr best_match = computeBestSqDist(_ftr_I->getPoints(), // <--feature incoming points
- _ftr_I->isFirstDefined(),
- _ftr_I->isLastDefined(),
- false, // <--feature is not closed for sure
- last_expected_points, // <--feature last points
- _ftr_L->isFirstDefined(),
- _ftr_L->isLastDefined(),
- false, // <--feature is not closed for sure
- params_tracker_feature_polyline_->match_feature_pose_sq_norm_th);
+ MatchPolyline2DPtr best_match = nullptr;
+
+ if (params_tracker_feature_polyline_->use_probabilistic_match)
+ {
+ // Compute Covariance of projected points (only considering noise in T_last_incoming) and computing Jacobian using the mean of the points
+ Eigen::Vector2s mean_last_points = _ftr_L->getPoints().rowwise().mean().head(2);
+ Eigen::Vector2s J_th;
+ Scalar cos_th = _T_last_incoming_prior(0,0);
+ Scalar sin_th = _T_last_incoming_prior(1,0);
+ J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1),
+ cos_th * mean_last_points(0) - sin_th * mean_last_points(1);
+ Eigen::Matrix2s Sigma = Eigen::Matrix2s::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_th +
+ J_th*params_tracker_feature_polyline_->match_feature_orientation_sq_norm_th*J_th.transpose();
+
+ // sq root of inverse
+ Eigen::Matrix2s sqrtOmega = Sigma.llt().matrixL().inverse();
+
+ // last expected points
+ Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+
+ // Shape points in the equiprobabilistic space
+ Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_shaped = last_expected_points;
+ Eigen::Matrix<Scalar, 3, Eigen::Dynamic> inc_shaped = _ftr_I->getPoints();
+ last_shaped.topRows<2>() = sqrtOmega*last_shaped.topRows<2>();
+ inc_shaped.topRows<2>() = sqrtOmega*inc_shaped.topRows<2>();
+
+ // compute best sq Mahalanobis distance matching
+ best_match = computeBestSqDist(inc_shaped, // <--feature incoming points
+ _ftr_I->isFirstDefined(),
+ _ftr_I->isLastDefined(),
+ false, // <--feature is not closed for sure
+ last_shaped, // <--feature last points
+ _ftr_L->isFirstDefined(),
+ _ftr_L->isLastDefined(),
+ false, // <--feature is not closed for sure
+ 1);
+ }
+ else // not probabilistic match
+ {
+ // last expected points
+ Eigen::Matrix<Scalar, 3, Eigen::Dynamic> last_expected_points = _T_last_incoming_prior.inverse() * _ftr_L->getPoints();
+
+ // compute best sq Mahalanobis distance matching
+ best_match = computeBestSqDist(_ftr_I->getPoints(), // <--feature incoming points
+ _ftr_I->isFirstDefined(),
+ _ftr_I->isLastDefined(),
+ false, // <--feature is not closed for sure
+ last_expected_points, // <--feature last points
+ _ftr_L->isFirstDefined(),
+ _ftr_L->isLastDefined(),
+ false, // <--feature is not closed for sure
+ params_tracker_feature_polyline_->match_feature_position_sq_norm_th);
+ }
+
//valid match
if (best_match != nullptr)
{
@@ -932,17 +975,60 @@ void ProcessorTrackerFeaturePolyline2D::tryMatchWithLandmark(LandmarkMatchPolyli
{
//WOLF_DEBUG("PTFP ", getName(), "::tryMatchWithLandmark: ");
- // compute best sq distance matching
- Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
- MatchPolyline2DPtr best_match = computeBestSqDist(lmk_expected_points, // <--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_landmark_pose_sq_norm_th);
+ MatchPolyline2DPtr best_match = nullptr;
+
+ if (params_tracker_feature_polyline_->use_probabilistic_match)
+ {
+ // Compute Covariance of projected points (only considering noise in _T_feature_landmark_prior) and computing Jacobian using the mean of the points
+ Eigen::Vector2s mean_last_points = _lmk_ptr->computePointsGlobal().rowwise().mean().head(2);
+ Eigen::Vector2s J_th;
+ Scalar cos_th = _T_feature_landmark_prior(0,0);
+ Scalar sin_th = _T_feature_landmark_prior(1,0);
+ J_th << -sin_th * mean_last_points(0) - cos_th * mean_last_points(1),
+ cos_th * mean_last_points(0) - sin_th * mean_last_points(1);
+ Eigen::Matrix2s Sigma = Eigen::Matrix2s::Identity()*params_tracker_feature_polyline_->match_feature_position_sq_norm_th +
+ J_th*params_tracker_feature_polyline_->match_feature_orientation_sq_norm_th*J_th.transpose();
+
+ // sq root of inverse
+ Eigen::Matrix2s sqrtOmega = Sigma.llt().matrixL().inverse();
+
+ // landmark expected points
+ Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+
+ // Shape points in the equiprobabilistic space
+ Eigen::Matrix<Scalar, 3, Eigen::Dynamic> lmk_shaped = lmk_expected_points;
+ Eigen::Matrix<Scalar, 3, Eigen::Dynamic> feat_shaped = _feat_ptr->getPoints();
+ lmk_shaped.topRows<2>() = sqrtOmega*lmk_shaped.topRows<2>();
+ feat_shaped.topRows<2>() = sqrtOmega*feat_shaped.topRows<2>();
+
+ // compute best sq Mahalanobis distance matching
+ best_match = computeBestSqDist(lmk_shaped, // <--landmark points in local coordinates
+ _lmk_ptr->isFirstDefined(),
+ _lmk_ptr->isLastDefined(),
+ _lmk_ptr->isClosed(),
+ feat_shaped, // <--feature last points
+ _feat_ptr->isFirstDefined(),
+ _feat_ptr->isLastDefined(),
+ false, // <--feature is not closed for sure
+ 1);
+ }
+ else // not probabilistic match
+ {
+ // landmark expected points
+ Eigen::MatrixXs lmk_expected_points = _T_feature_landmark_prior * _lmk_ptr->computePointsGlobal();
+
+ // compute best sq distance matching
+ best_match = computeBestSqDist(lmk_expected_points, // <--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_landmark_pose_sq_norm_th);
+ }
+
//valid match
if (best_match != nullptr)
{