diff --git a/CMakeLists.txt b/CMakeLists.txt
index 447618a490642e374e5745e5164470456f626bf8..c6e0000f09b4f33d171452c1cf93755cd6735a58 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -416,6 +416,7 @@ SET(HDRS_PROCESSOR
include/base/processor/processor_gnss_fix.h
include/base/processor/processor_gnss_single_diff.h
include/base/processor/processor_tracker_feature_polyline.h
+ include/base/processor/polyline_2D_utils.h
)
SET(HDRS_SENSOR
include/base/sensor/sensor_base.h
@@ -569,6 +570,7 @@ SET(SRCS_PROCESSOR
src/processor/processor_gnss_fix.cpp
src/processor/processor_gnss_single_diff.cpp
src/processor/processor_tracker_feature_polyline.cpp
+ src/processor/polyline_2D_utils.cpp
)
SET(SRCS_SENSOR
src/sensor/sensor_camera.cpp
diff --git a/include/base/feature/feature_match_polyline_2D.h b/include/base/feature/feature_match_polyline_2D.h
index 38624b6f3c346ab581cce34662c20d51732dbcc6..6e3a6752831a448b456b7efa57442fb6969f9218 100644
--- a/include/base/feature/feature_match_polyline_2D.h
+++ b/include/base/feature/feature_match_polyline_2D.h
@@ -22,6 +22,7 @@ struct FeatureMatchPolyline2D : public FeatureMatch
int feature_last_to_id_;
int feature_incoming_from_id_;
int feature_incoming_to_id_;
+ Eigen::Matrix3s T_incoming_last_;
};
}//end namespace
diff --git a/include/base/feature/feature_polyline_2D.h b/include/base/feature/feature_polyline_2D.h
index 4974de6501335f3276d7496868d94aa146bc7b77..071f46be51102ca3384cc1ba967d3817e4f8731e 100644
--- a/include/base/feature/feature_polyline_2D.h
+++ b/include/base/feature/feature_polyline_2D.h
@@ -19,7 +19,7 @@ WOLF_LIST_TYPEDEFS(FeaturePolyline2D);
class FeaturePolyline2D : public FeatureBase
{
protected:
- Eigen::MatrixXs points_;
+ Eigen::MatrixXs points_; // homogeneous 2D points (each column is a point)
Eigen::MatrixXs points_cov_;
bool first_defined_;
bool last_defined_;
diff --git a/include/base/landmark/landmark_match_polyline_2D.h b/include/base/landmark/landmark_match_polyline_2D.h
index 3298c99645a3d0d157795e49adb52c85f72a1f1e..f7835b0c87005769acf7ffd96356906699bf3f7c 100644
--- a/include/base/landmark/landmark_match_polyline_2D.h
+++ b/include/base/landmark/landmark_match_polyline_2D.h
@@ -24,6 +24,7 @@ struct LandmarkMatchPolyline2D : public LandmarkMatch
int landmark_to_id_;
int feature_to_id_;
int landmark_version_;
+ Eigen::Matrix3s T_feature_landmark_;
};
}//end namespace
diff --git a/include/base/processor/polyline_2D_utils.h b/include/base/processor/polyline_2D_utils.h
index 947db7d7028739e0f9df6c73469d1b0f99204b31..7db4cbaf50e6c4b59d401dfda45d2620d95fdd1a 100644
--- a/include/base/processor/polyline_2D_utils.h
+++ b/include/base/processor/polyline_2D_utils.h
@@ -13,344 +13,48 @@
#include "base/landmark/landmark_polyline_2D.h"
#include "base/feature/feature_match_polyline_2D.h"
#include "base/feature/feature_polyline_2D.h"
+#include "base/rotations.h"
namespace wolf {
-/** \brief Computes the squared distance from a point to a segment defined by two points
+WOLF_STRUCT_PTR_TYPEDEFS(MatchPolyline2D);
+
+/** \brief Match between a two polylines (not specialized to landmark nor feature)
+ *
+ * Match between a two polylines (not specialized to landmark nor feature)
+ *
**/
-Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b)
+struct MatchPolyline2D
{
- Scalar ap_sq = (p-a).squaredNorm();
- Scalar bp_sq = (p-b).squaredNorm();
- Scalar ab_sq = (b-a).squaredNorm();
- Scalar sqDist;
+ int from_A_id_;
+ int to_A_id_;
+ int from_B_id_;
+ int to_B_id_;
+ int normalized_score_;
+};
- // If the projection of p onto ab is inside the segment (acute triangle: all angles < 90º), return the sq distance point-line.
- if (ap_sq <= bp_sq + ab_sq && bp_sq <= ap_sq + ab_sq) // acute triangle: applying pitagoras twice, angles ABP and PAB <= 90º
- sqDist = ap_sq - std::pow((b-a).dot(p-a),2) / ab_sq;
+Eigen::Matrix3s transMatrix2D(const Eigen::Vector2s& t, const Scalar& theta);
- // Otherwise, return the sq distance to the closest segment point.
- else
- sqDist = std::min(bp_sq,ap_sq);
+Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last);
- return sqDist;
-}
+/** \brief Computes the squared distance from a point to a segment defined by two points
+ **/
+Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b);
Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict=false)
-{
- /* 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 & A not defined
- *
- * If projection of B over AAaux is in [A_aux, inf), return sq.dist. B-Line(AAux)
- * Otherwise, return sqDist(Aaux-B) or 1e12 if strict=true.
- *
- * Check: Angle BAauxA is <= 90º: (AB)² <= (AauxB)² + (AAaux)²
- *
- * Case 2: B not defined & A defined
- *
- * If projection of B over AAaux is in [A_aux, A], return sq.dist. B-Line(AAux)
- * Otherwise, return min(sqDist(Aaux-B),sqDist(A-B)) or 1e12 if strict=true.
- *
- * Check: Angles BAauxA & BAAaux are <= 90º: (AB)² <= (AauxB)² + (AAaux)² & (AauxB)² <= (AB)² + (AAaux)²
- *
- * Case 3: B defined & A not defined
- *
- * If projection of B over AAaux is in [A, inf), return sq.dist. B-Line(AAux)
- * Otherwise, return sqDist(A-B)) or 1e12 if strict=true.
- *
- * Check: Angle BAAaux is >= 90º: (AauxB)² >= (AB)² + (AAaux)²
- *
- * (both points defined is not point-to-line, is point-to-point)
- *
- */
-
- assert((!_A_defined || !_B_defined) && "ProcessorPolyline::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();
-
- // squared distance to line
- // Case 1
- if (!_B_defined && !_A_defined)
- {
- if (AB_sq <= AauxB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq;
- else
- return (strict ? 1e12 : AauxB_sq);
- }
-
- // Case 2
- if (!_B_defined && _A_defined)
- {
- if (AB_sq <= AauxB_sq + AAaux_sq && AauxB_sq <= AB_sq + AAaux_sq)
- return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq;
- else
- return (strict ? 1e12 : std::min(AauxB_sq,AB_sq));
- }
-
- // Case 3
- if (_B_defined && !_A_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;
- else
- return (strict ? 1e12 : AB_sq);
- }
-
- // Default return unused
- return 1e12;
-}
-
-LandmarkMatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
- const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
- const Scalar& max_sq_error)
-{
- //std::cout << "computeBestSqDist...\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 << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
- //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
-
- // ASSERTIONS if closed -> all points defined
- assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
- assert((_closed_B &&_first_defined_B && _last_defined_B) || !_closed_B);
-
- /* ASSUMPTIONS:
- *
- * IF BOTH NOT CLOSED:
- * (a) - A has equal or more points than B --> Otherwise: Call the function switching A<->B
- *
- * IF ONE CLOSED:
- * (b) - Should be A closed (not B) --> Otherwise: Call the function switching A<->B
- * (c) - B.defined_points <= A.(defined_)points --> Otherwise: No matching
- *
- * IF BOTH CLOSED:
- * (d) - B.(defined_)points == A.(defined_)points --> Otherwise: No matching
- */
-
- LandmarkMatchPolyline2DPtr best_match = nullptr;
-
- // ----------------- Call switching A<->B
- if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
- (!_closed_A && _closed_B) ) // (b)
- {
- //std::cout << "Auto-calling switching A<->B...\n";
-
- LandmarkMatchPolyline2DPtr best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
- _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error);
- // undo switching
- if (best_match != nullptr)
- {
- std::swap(best_match->feature_from_id_, best_match->landmark_from_id_);
- std::swap(best_match->feature_to_id_, best_match->landmark_to_id_);
- }
- return best_match;
- }
-
- // ------------------ No matching (c) & (d)
- int N_defined_B = _points_B.cols() - (_first_defined_B ? 0 : 1) - (_last_defined_B ? 0 : 1);
-
- if ((_closed_A && !_closed_B && _points_A.cols() < N_defined_B) || // (c)
- (_closed_A && _closed_B && _points_A.cols() != _points_B.cols())) // (d)
- return nullptr;
-
- /* ------------------ Normal function call:
- * Search along all possible overlapping configurations between A&B points
- * We define offset as the correspondence (if exists) of the first point_A to B's.
- * Offset is within [min_offset, max_offset], we distinguish between 3 cases:
- * 1. None closed: [-last_A, last_B]
- * 2. A closed: [-last_A, 0]
- */
-
- int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlapped;
- last_A = _points_A.cols() - 1;
- last_B = _points_B.cols() - 1;
+ const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict=false);
- if (!_closed_A)
- {
- min_offset = -last_A;
- max_offset = last_B;
- }
- else
- {
- min_offset = -last_A;
- max_offset = 0;
- }
+MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const Scalar& max_sq_error );
- // Check all overlapping configurations
- for (offset = min_offset; offset <= max_offset; offset++)
- {
- from_A = std::max(0,-offset);
- from_B = std::max(0, offset);
-
- if (!_closed_A && !_closed_B)
- N_overlapped = std::min(last_B - from_B, last_A - from_A)+1;
- else if(_closed_A)
- N_overlapped = _points_B.cols();
- else
- std::runtime_error("Impossible state. Function should auto-called reversing A&B");
-
- to_A = from_A+N_overlapped-1;
- to_B = from_B+N_overlapped-1;
-
- while (to_A > last_A && _closed_A)
- to_A -= _points_A.cols();
-
- //std::cout << "\toffset " << offset << std::endl;
- //std::cout << "\t\tfrom_A " << from_A << std::endl;
- //std::cout << "\t\tto_A " << to_A << std::endl;
- //std::cout << "\t\tfrom_B " << from_B << std::endl;
- //std::cout << "\t\tto_B " << to_B << std::endl;
- //std::cout << "\t\tlast_A " << last_A << std::endl;
- //std::cout << "\t\tlast_B " << last_B << std::endl;
- //std::cout << "\t\tN_overlapped " << N_overlapped << std::endl;
-
- bool from_A_defined = ((from_A != 0 && from_A != last_A) || _first_defined_A);
- bool from_B_defined = ((from_B != 0 && from_B != last_B) || _first_defined_B);
- bool to_A_defined = ((to_A != 0 && to_A != last_A) || _last_defined_A);
- bool to_B_defined = ((to_B != 0 && to_B != last_B) || _last_defined_B);
-
- //std::cout << "\t\tfrom_A_defined " << from_A_defined << (from_A == 0) << (from_A == last_A) << _first_defined_A << std::endl;
- //std::cout << "\t\tfrom_B_defined " << from_B_defined << (from_B == 0) << (from_B == last_B) << _first_defined_B << std::endl;
- //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 &&
- (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
- continue;
-
- assert(N_overlapped <= _points_B.cols());
- assert(N_overlapped <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlapped can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
- assert(from_A >= 0);
- assert(from_B >= 0);
- assert(from_B == 0 || !_closed_A);
- assert(to_B == _points_B.cols()-1 || !_closed_A);
- assert(to_A <= last_A);
- assert(to_B <= last_B);
- assert(to_A < _points_A.cols());
- assert(to_B < _points_B.cols());
-
- // POINT-POINT DISTANCES for all overlapped points (in case of closed A in two parts)
- // Fill with B points
- Eigen::ArrayXXd d(_points_B.block(0,from_B, 2, N_overlapped).array());
-
- // Substract A points
- if (to_A >= from_A && N_overlapped <= _points_A.cols())
- d -= _points_A.block(0,from_A, 2, N_overlapped).array();
- else
- {
- d.leftCols(_points_A.cols()-from_A) -= _points_A.block(0,from_A, 2, _points_A.cols()-from_A).array();
- d.rightCols(to_A+1) -= _points_A.block(0, 0, 2, to_A+1).array();
- }
- Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
-
- // CORRECT POINT-LINE DISTANCES for not defined extremes
- // First
- if (!from_B_defined || !from_A_defined)
- {
- // take care of closed A: next of from_A
- int next_from_A = (from_A+1 > last_A ? 0 : from_A+1);
- //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
-
- // std::cout << "\t\t\tA " << _points_A.col(from_A).transpose() << std::endl;
- // std::cout << "\t\t\tAaux" << _points_A.col(next_from_A).transpose() << std::endl;
- // std::cout << "\t\t\tB " << _points_B.col(from_B).transpose() << std::endl;
- dist2(0) = sqDistPointToLine(_points_A.col(from_A),
- _points_A.col(next_from_A),
- _points_B.col(from_B),
- from_A_defined,
- from_B_defined);
- assert(N_overlapped > 1);
- }
- // Last
- if (!to_B_defined || !to_A_defined)
- {
- // take care of closed A: next of to_A
- int prev_to_A = (to_A == 0 ? last_A : to_A-1);
- //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
-
- // std::cout << "\t\t\tA " << _points_A.col(to_A).transpose() << std::endl;
- // std::cout << "\t\t\tAaux" << _points_A.col(prev_to_A).transpose() << std::endl;
- // std::cout << "\t\t\tB " << _points_B.col(to_B).transpose() << std::endl;
- dist2(N_overlapped-1) = sqDistPointToLine(_points_A.col(to_A),
- _points_A.col(prev_to_A),
- _points_B.col(to_B),
- to_A_defined,
- to_B_defined);
- assert(N_overlapped > 1);
- }
- //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be within a threshold
- // Choose the most overlapped one
- if ((dist2 < max_sq_error).all() &&
- (best_match == nullptr ||
- (N_overlapped >= best_match->feature_to_id_-best_match->feature_from_id_+1 &&
- dist2.mean() < best_match->normalized_score_ )))
- {
- //std::cout << "\tBEST MATCH" << std::endl;
- if (best_match == nullptr)
- best_match = std::make_shared<LandmarkMatchPolyline2D>();
-
- best_match->feature_from_id_= from_B;
- best_match->feature_to_id_= to_B;
- best_match->landmark_ptr_= nullptr;
- best_match->normalized_score_ = dist2.mean();
- best_match->landmark_from_id_= from_A;
- best_match->landmark_to_id_= to_A;
- }
- }
-
- return best_match;
-}
-
-bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B)
-{
- /* The orthogonal projection of B over the line A-Aaux is in the segment [A,Aaux].
- *
- * Check: the angles BAAaux and BAauxA are <= 90º:
- * (BAaux)² <= (BA)² + (AAaux)² & (BA)² <= (BAaux)² + (AAaux)²
- *
- */
-
- 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();
-
- return (AauxB_sq <= AB_sq + AAaux_sq) && (AB_sq <= AauxB_sq + AAaux_sq);
-}
+bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B);
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)
-{
- // ------------------------ 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;
-}
+ const Eigen::MatrixXs& _mu);
} // namespace wolf
diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline.h
index dbc701e3bef4257f9e05e537e64a0c0edc4e2500..78c24b7d9c82f06517e14b5e91893ab542f68814 100644
--- a/include/base/processor/processor_tracker_feature_polyline.h
+++ b/include/base/processor/processor_tracker_feature_polyline.h
@@ -166,10 +166,10 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
*/
virtual void postProcess() override;
- FeatureMatchPolyline2DPtr computeBestMatch(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
+ FeatureMatchPolyline2DPtr tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
- FeatureMatchPolyline2DPtr bestMatch(const FeatureMatchPolyline2DPtr& _match_A,
- const FeatureMatchPolyline2DPtr& _match_B) const;
+ FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A,
+ const FeatureMatchPolyline2DPtr& _match_B) const;
LandmarkMatchPolyline2DPtr tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index 3694b28eb19b735285fe5a17a60eeabcb42b80a3..69e75eb51ee0d98a3587a066e9a4b44416ed2aaa 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -975,67 +975,67 @@ void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_to
// 2 by 2
LandmarkPolyline2DPtr remaining_lmk = _lmk_list.front();
_lmk_list.pop_front();
- LandmarkPolyline2DPtr A_lmk, B_lmk, merged_lmk;
+ LandmarkPolyline2DPtr lmk_1, lmk_2, merged_lmk;
int remaining_from, remaining_to, merged_from, merged_to;
while (!_lmk_list.empty())
{
- A_lmk = remaining_lmk;
- B_lmk = _lmk_list.front();
+ lmk_1 = remaining_lmk;
+ lmk_2 = _lmk_list.front();
_lmk_list.pop_front();
- if (B_lmk->isRemoving())
+ if (lmk_2->isRemoving())
continue;
- std::cout << "Candidates to be merged: " << A_lmk->id() << " " << B_lmk->id() << "\n";
+ std::cout << "Candidates to be merged: " << lmk_1->id() << " " << lmk_2->id() << "\n";
// check best correspondence
- Eigen::MatrixXs points_A = A_lmk->computePointsGlobal();
- Eigen::MatrixXs points_B = B_lmk->computePointsGlobal();
- LandmarkMatchPolyline2DPtr match = computeBestSqDist(points_A, A_lmk->isFirstDefined(), A_lmk->isLastDefined(), A_lmk->isClosed(),
- points_B, B_lmk->isFirstDefined(), B_lmk->isLastDefined(), B_lmk->isClosed(), _dist_tol*_dist_tol);
+ Eigen::MatrixXs points_1 = lmk_1->computePointsGlobal();
+ Eigen::MatrixXs points_2 = lmk_2->computePointsGlobal();
+ MatchPolyline2DPtr match = computeBestSqDist(points_1, lmk_1->isFirstDefined(), lmk_1->isLastDefined(), lmk_1->isClosed(),
+ points_2, lmk_2->isFirstDefined(), lmk_2->isLastDefined(), lmk_2->isClosed(), _dist_tol*_dist_tol);
// match
if ( match )
{
- std::vector<int> valid_id_A = A_lmk->getValidIds();
- std::vector<int> valid_id_B = B_lmk->getValidIds();
+ std::vector<int> valid_id_1 = lmk_1->getValidIds();
+ std::vector<int> valid_id_2 = lmk_2->getValidIds();
// decide which is the remaining landmark:
// If only one closed, this one remains.
// Otherwise (both closed or both not closed) the one with most constraints remains
- if ((A_lmk->isClosed() && !B_lmk->isClosed()) ||
- (A_lmk->isClosed() == B_lmk->isClosed() && A_lmk->getHits() > B_lmk->getHits()))
+ if ((lmk_1->isClosed() && !lmk_2->isClosed()) ||
+ (lmk_1->isClosed() == lmk_2->isClosed() && lmk_1->getHits() > lmk_2->getHits()))
{
- remaining_lmk = A_lmk;
- merged_lmk = B_lmk;
- remaining_from = valid_id_A.at(match->landmark_from_id_);
- remaining_to = valid_id_A.at(match->landmark_to_id_);
- merged_from = valid_id_B.at(match->feature_from_id_);
- merged_to = valid_id_B.at(match->feature_to_id_);
+ remaining_lmk = lmk_1;
+ merged_lmk = lmk_2;
+ remaining_from = valid_id_1.at(match->from_A_id_);
+ remaining_to = valid_id_1.at(match->to_A_id_);
+ merged_from = valid_id_2.at(match->from_B_id_);
+ merged_to = valid_id_2.at(match->to_B_id_);
}
else
{
- remaining_lmk = B_lmk;
- merged_lmk = A_lmk;
- remaining_from = valid_id_B.at(match->feature_from_id_);
- remaining_to = valid_id_B.at(match->feature_to_id_);
- merged_from = valid_id_A.at(match->landmark_from_id_);
- merged_to = valid_id_A.at(match->landmark_to_id_);
+ remaining_lmk = lmk_2;
+ merged_lmk = lmk_1;
+ remaining_from = valid_id_2.at(match->from_B_id_);
+ remaining_to = valid_id_2.at(match->to_B_id_);
+ merged_from = valid_id_1.at(match->from_A_id_);
+ merged_to = valid_id_1.at(match->to_A_id_);
}
// merge 2 landmarks
remaining_lmk->mergeLandmark(merged_lmk, remaining_from, remaining_to, merged_from, merged_to);
// reset pointers
- A_lmk.reset();
- B_lmk.reset();
+ lmk_1.reset();
+ lmk_2.reset();
assert(merged_lmk->getHits() == 0);
std::cout << "merged\n";
}
// no match (remaining: most constraints)
else
- remaining_lmk = (A_lmk->getHits() > B_lmk->getHits() ? A_lmk : B_lmk);
+ remaining_lmk = (lmk_1->getHits() > lmk_2->getHits() ? lmk_1 : lmk_2);
}
std::cout << "tryMergeLandmarks::done\n";
}
diff --git a/src/processor/polyline_2D_utils.cpp b/src/processor/polyline_2D_utils.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..23b021a9158707c79f12db8be876151bc4e176a4
--- /dev/null
+++ b/src/processor/polyline_2D_utils.cpp
@@ -0,0 +1,383 @@
+#include "base/processor/polyline_2D_utils.h"
+
+namespace wolf {
+
+Eigen::Matrix3s transMatrix2D(const Eigen::Vector2s& t, const Scalar& theta)
+{
+ Eigen::Matrix3s T(Eigen::Matrix3s::Identity());
+
+ T.topLeftCorner(2,2) = Eigen::Rotation2D<Scalar>(theta).matrix();
+ T.topRightCorner(2,1) = t;
+
+ return T;
+}
+
+Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_incoming, const Eigen::MatrixXs& _points_last)
+{
+ assert(_points_incoming.cols() == _points_last.cols());
+ assert(_points_incoming.rows() >= 2);
+ assert(_points_last.rows() >= 2);
+
+ Eigen::Vector3s t_incoming_last;
+
+ // Rotation
+ Eigen::VectorXs angles_incoming_last(_points_incoming.cols());
+ Eigen::MatrixXs points_incoming_mean = _points_incoming.colwise()-_points_incoming.rowwise().mean();
+ Eigen::MatrixXs points_last_mean = _points_last.colwise()-_points_last.rowwise().mean();
+
+ for (auto i = 0; i<_points_incoming.cols(); i++)
+ angles_incoming_last(i) = pi2pi(atan2(points_incoming_mean(1,i),points_incoming_mean(0,i)) - atan2(points_last_mean(1,i),points_last_mean(0,i)));
+
+ // fix 2nd&3rd quadrant angles bad resulting mean
+ if (angles_incoming_last.maxCoeff() > M_PI / 2 && angles_incoming_last.minCoeff() < -M_PI / 2)
+ angles_incoming_last = (angles_incoming_last.array() > 0).select(angles_incoming_last, angles_incoming_last.array()+2*M_PI);
+
+ t_incoming_last(2) = angles_incoming_last.mean();
+
+ // Translation
+ t_incoming_last.head<2>() = _points_incoming.topRows<2>().rowwise().mean() - Eigen::Rotation2D<Scalar>(t_incoming_last(2))*_points_last.topRows<2>().rowwise().mean();
+
+
+ return t_incoming_last;
+}
+
+/** \brief Computes the squared distance from a point to a segment defined by two points
+ **/
+Scalar sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b)
+{
+ Scalar ap_sq = (p-a).squaredNorm();
+ Scalar bp_sq = (p-b).squaredNorm();
+ Scalar ab_sq = (b-a).squaredNorm();
+ Scalar sqDist;
+
+ // If the projection of p onto ab is inside the segment (acute triangle: all angles < 90º), return the sq distance point-line.
+ if (ap_sq <= bp_sq + ab_sq && bp_sq <= ap_sq + ab_sq) // acute triangle: applying pitagoras twice, angles ABP and PAB <= 90º
+ sqDist = ap_sq - std::pow((b-a).dot(p-a),2) / ab_sq;
+
+ // Otherwise, return the sq distance to the closest segment point.
+ else
+ sqDist = std::min(bp_sq,ap_sq);
+
+ return sqDist;
+}
+
+Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
+ const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict)
+{
+ /* 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 & A not defined
+ *
+ * If projection of B over AAaux is in [A_aux, inf), return sq.dist. B-Line(AAux)
+ * Otherwise, return sqDist(Aaux-B) or 1e12 if strict=true.
+ *
+ * Check: Angle BAauxA is <= 90º: (AB)² <= (AauxB)² + (AAaux)²
+ *
+ * Case 2: B not defined & A defined
+ *
+ * If projection of B over AAaux is in [A_aux, A], return sq.dist. B-Line(AAux)
+ * Otherwise, return min(sqDist(Aaux-B),sqDist(A-B)) or 1e12 if strict=true.
+ *
+ * Check: Angles BAauxA & BAAaux are <= 90º: (AB)² <= (AauxB)² + (AAaux)² & (AauxB)² <= (AB)² + (AAaux)²
+ *
+ * Case 3: B defined & A not defined
+ *
+ * If projection of B over AAaux is in [A, inf), return sq.dist. B-Line(AAux)
+ * Otherwise, return sqDist(A-B)) or 1e12 if strict=true.
+ *
+ * Check: Angle BAAaux is >= 90º: (AauxB)² >= (AB)² + (AAaux)²
+ *
+ * (both points defined is not point-to-line, is point-to-point)
+ *
+ */
+
+ assert((!_A_defined || !_B_defined) && "ProcessorPolyline::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();
+
+ // squared distance to line
+ // Case 1
+ if (!_B_defined && !_A_defined)
+ {
+ if (AB_sq <= AauxB_sq + AAaux_sq)
+ return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq;
+ else
+ return (strict ? 1e12 : AauxB_sq);
+ }
+
+ // Case 2
+ if (!_B_defined && _A_defined)
+ {
+ if (AB_sq <= AauxB_sq + AAaux_sq && AauxB_sq <= AB_sq + AAaux_sq)
+ return AB_sq - std::pow(((_A_aux-_A).head<2>()).dot((_B-_A).head<2>()),2) / AAaux_sq;
+ else
+ return (strict ? 1e12 : std::min(AauxB_sq,AB_sq));
+ }
+
+ // Case 3
+ if (_B_defined && !_A_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;
+ else
+ return (strict ? 1e12 : AB_sq);
+ }
+
+ // Default return unused
+ return 1e12;
+}
+
+MatchPolyline2DPtr computeBestSqDist(const Eigen::MatrixXs& _points_A, bool _first_defined_A, bool _last_defined_A, bool _closed_A,
+ const Eigen::MatrixXs& _points_B, bool _first_defined_B, bool _last_defined_B, bool _closed_B,
+ const Scalar& max_sq_error )
+{
+ //std::cout << "computeBestSqDist...\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 << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
+ //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
+
+ // ASSERTIONS if closed -> all points defined
+ assert((_closed_A &&_first_defined_A && _last_defined_A) || !_closed_A);
+ assert((_closed_B &&_first_defined_B && _last_defined_B) || !_closed_B);
+
+ /* ASSUMPTIONS:
+ *
+ * IF BOTH NOT CLOSED:
+ * (a) - A has equal or more points than B --> Otherwise: Call the function switching A<->B
+ *
+ * IF ONE CLOSED:
+ * (b) - Should be A closed (not B) --> Otherwise: Call the function switching A<->B
+ * (c) - B.defined_points <= A.(defined_)points --> Otherwise: No matching
+ *
+ * IF BOTH CLOSED:
+ * (d) - B.(defined_)points == A.(defined_)points --> Otherwise: No matching
+ */
+
+ MatchPolyline2DPtr best_match = nullptr;
+
+ // ----------------- Call switching A<->B
+ if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
+ (!_closed_A && _closed_B) ) // (b)
+ {
+ //std::cout << "Auto-calling switching A<->B...\n";
+
+ MatchPolyline2DPtr best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
+ _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error);
+ // undo switching
+ if (best_match != nullptr)
+ {
+ std::swap(best_match->from_A_id_, best_match->from_B_id_);
+ std::swap(best_match->to_A_id_, best_match->to_B_id_);
+ }
+ return best_match;
+ }
+
+ // ------------------ No matching (c) & (d)
+ int N_defined_B = _points_B.cols() - (_first_defined_B ? 0 : 1) - (_last_defined_B ? 0 : 1);
+
+ if ((_closed_A && !_closed_B && _points_A.cols() < N_defined_B) || // (c)
+ (_closed_A && _closed_B && _points_A.cols() != _points_B.cols())) // (d)
+ return nullptr;
+
+ /* ------------------ Normal function call:
+ * Search along all possible overlapping configurations between A&B points
+ * We define offset as the correspondence (if exists) of the first point_A to B's.
+ * Offset is within [min_offset, max_offset], we distinguish between 3 cases:
+ * 1. None closed: [-last_A, last_B]
+ * 2. A closed: [-last_A, 0]
+ */
+
+ int last_A, last_B, offset, max_offset, min_offset, from_A, to_A, from_B, to_B, N_overlapped;
+ last_A = _points_A.cols() - 1;
+ last_B = _points_B.cols() - 1;
+
+ if (!_closed_A)
+ {
+ min_offset = -last_A;
+ max_offset = last_B;
+ }
+ else
+ {
+ min_offset = -last_A;
+ max_offset = 0;
+ }
+
+ // Check all overlapping configurations
+ for (offset = min_offset; offset <= max_offset; offset++)
+ {
+ from_A = std::max(0,-offset);
+ from_B = std::max(0, offset);
+
+ if (!_closed_A && !_closed_B)
+ N_overlapped = std::min(last_B - from_B, last_A - from_A)+1;
+ else if(_closed_A)
+ N_overlapped = _points_B.cols();
+ else
+ std::runtime_error("Impossible state. Function should auto-called reversing A&B");
+
+ to_A = from_A+N_overlapped-1;
+ to_B = from_B+N_overlapped-1;
+
+ while (to_A > last_A && _closed_A)
+ to_A -= _points_A.cols();
+
+ //std::cout << "\toffset " << offset << std::endl;
+ //std::cout << "\t\tfrom_A " << from_A << std::endl;
+ //std::cout << "\t\tto_A " << to_A << std::endl;
+ //std::cout << "\t\tfrom_B " << from_B << std::endl;
+ //std::cout << "\t\tto_B " << to_B << std::endl;
+ //std::cout << "\t\tlast_A " << last_A << std::endl;
+ //std::cout << "\t\tlast_B " << last_B << std::endl;
+ //std::cout << "\t\tN_overlapped " << N_overlapped << std::endl;
+
+ bool from_A_defined = ((from_A != 0 && from_A != last_A) || _first_defined_A);
+ bool from_B_defined = ((from_B != 0 && from_B != last_B) || _first_defined_B);
+ bool to_A_defined = ((to_A != 0 && to_A != last_A) || _last_defined_A);
+ bool to_B_defined = ((to_B != 0 && to_B != last_B) || _last_defined_B);
+
+ //std::cout << "\t\tfrom_A_defined " << from_A_defined << (from_A == 0) << (from_A == last_A) << _first_defined_A << std::endl;
+ //std::cout << "\t\tfrom_B_defined " << from_B_defined << (from_B == 0) << (from_B == last_B) << _first_defined_B << std::endl;
+ //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 &&
+ (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
+ continue;
+
+ assert(N_overlapped <= _points_B.cols());
+ assert(N_overlapped <= _points_A.cols() || (_closed_A && N_defined_B < _points_B.cols())); // N_overlapped can be > _points_A if all defined points of B match with A points and the not defined extremes should match too
+ assert(from_A >= 0);
+ assert(from_B >= 0);
+ assert(from_B == 0 || !_closed_A);
+ assert(to_B == _points_B.cols()-1 || !_closed_A);
+ assert(to_A <= last_A);
+ assert(to_B <= last_B);
+ assert(to_A < _points_A.cols());
+ assert(to_B < _points_B.cols());
+
+ // POINT-POINT DISTANCES for all overlapped points (in case of closed A in two parts)
+ // Fill with B points
+ Eigen::ArrayXXd d(_points_B.block(0,from_B, 2, N_overlapped).array());
+
+ // Substract A points
+ if (to_A >= from_A && N_overlapped <= _points_A.cols())
+ d -= _points_A.block(0,from_A, 2, N_overlapped).array();
+ else
+ {
+ d.leftCols(_points_A.cols()-from_A) -= _points_A.block(0,from_A, 2, _points_A.cols()-from_A).array();
+ d.rightCols(to_A+1) -= _points_A.block(0, 0, 2, to_A+1).array();
+ }
+ Eigen::ArrayXd dist2 = d.row(0).pow(2) + d.row(1).pow(2);
+
+ // CORRECT POINT-LINE DISTANCES for not defined extremes
+ // First
+ if (!from_B_defined || !from_A_defined)
+ {
+ // take care of closed A: next of from_A
+ int next_from_A = (from_A+1 > last_A ? 0 : from_A+1);
+ //std::cout << "\t\t\tFirst feature not defined distance to line" << std::endl;
+
+ // std::cout << "\t\t\tA " << _points_A.col(from_A).transpose() << std::endl;
+ // std::cout << "\t\t\tAaux" << _points_A.col(next_from_A).transpose() << std::endl;
+ // std::cout << "\t\t\tB " << _points_B.col(from_B).transpose() << std::endl;
+ dist2(0) = sqDistPointToLine(_points_A.col(from_A),
+ _points_A.col(next_from_A),
+ _points_B.col(from_B),
+ from_A_defined,
+ from_B_defined);
+ assert(N_overlapped > 1);
+ }
+ // Last
+ if (!to_B_defined || !to_A_defined)
+ {
+ // take care of closed A: next of to_A
+ int prev_to_A = (to_A == 0 ? last_A : to_A-1);
+ //std::cout << "\t\t\tLast feature not defined distance to line" << std::endl;
+
+ // std::cout << "\t\t\tA " << _points_A.col(to_A).transpose() << std::endl;
+ // std::cout << "\t\t\tAaux" << _points_A.col(prev_to_A).transpose() << std::endl;
+ // std::cout << "\t\t\tB " << _points_B.col(to_B).transpose() << std::endl;
+ dist2(N_overlapped-1) = sqDistPointToLine(_points_A.col(to_A),
+ _points_A.col(prev_to_A),
+ _points_B.col(to_B),
+ to_A_defined,
+ to_B_defined);
+ assert(N_overlapped > 1);
+ }
+ //std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
+
+ // All squared distances should be within a threshold
+ if ((dist2 < max_sq_error).all())
+ {
+ Scalar normalized_score = (max_sq_error - dist2.mean()) / max_sq_error;
+
+ // Choose the most overlapped match, if equally overlapped: the best score
+ if (best_match == nullptr ||
+ N_overlapped > best_match->to_A_id_-best_match->from_A_id_+1 ||
+ (N_overlapped == best_match->to_A_id_-best_match->from_A_id_+1 && normalized_score > best_match->normalized_score_ ))
+ {
+ //std::cout << "\tBEST MATCH" << std::endl;
+ if (best_match == nullptr)
+ best_match = std::make_shared<MatchPolyline2D>();
+
+ best_match->from_A_id_= from_A;
+ best_match->to_A_id_= to_A;
+ best_match->from_B_id_= from_B;
+ best_match->to_B_id_= to_B;
+ best_match->normalized_score_ = normalized_score;
+ }
+ }
+ }
+
+ return best_match;
+}
+
+bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B)
+{
+ /* The orthogonal projection of B over the line A-Aaux is in the segment [A,Aaux].
+ *
+ * Check: the angles BAAaux and BAauxA are <= 90º:
+ * (BAaux)² <= (BA)² + (AAaux)² & (BA)² <= (BAaux)² + (AAaux)²
+ *
+ */
+
+ 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();
+
+ return (AauxB_sq <= AB_sq + AAaux_sq) && (AB_sq <= AauxB_sq + AAaux_sq);
+}
+
+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)
+{
+ // ------------------------ 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;
+}
+
+} // namespace wolf
diff --git a/src/processor/processor_polyline.cpp b/src/processor/processor_polyline.cpp
index 890f7eccb812e599ccc09746d962a09ac3bd0fb8..901543c14c5603dd74fe1b4208c6b2f2b0bf96d9 100644
--- a/src/processor/processor_polyline.cpp
+++ b/src/processor/processor_polyline.cpp
@@ -468,8 +468,10 @@ void ProcessorPolyline::establishConstraints()
// CHECK IF LANDMARK CHANGED AFTER MATCHING
if (pl_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
{
- // get the not merged lmk
- pl_lmk = pl_lmk->getMergedInLandmark();
+ // get the lmk in which lmk was merged
+ auto merged_in_lmk = pl_lmk->getMergedInLandmark();
+ if (merged_in_lmk)
+ pl_lmk = merged_in_lmk;
// redo matching
LandmarkMatchPolyline2DPtr updated_match = tryMatch(pl_lmk, pl_ftr);
@@ -631,18 +633,14 @@ void ProcessorPolyline::establishConstraints()
}
else
{
+ // add recently created landmarks to the list of modified landmark
if (pl_lmk->getHits() == 0)
- {
- // add recently created landmarks to the list of modified landmark
modified_lmks.push_back(pl_lmk);
- assert(pl_match->feature_from_id_ == 0 && "recently created landmark with not full feature match");
- assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "recently created landmark with not full feature match");
- }
- if (pl_lmk->isClosed())
- {
- assert(pl_match->feature_from_id_ == 0 && "closed landmark with not full feature match");
- assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "closed landmark with not full feature match");
- }
+
+ assert((pl_lmk->getHits() != 0 || pl_match->feature_from_id_ == 0) && "recently created landmark with not full feature match");
+ assert((pl_lmk->getHits() != 0 || pl_match->feature_to_id_ == pl_ftr->getNPoints()-1) && "recently created landmark with not full feature match");
+ assert((!pl_lmk->isClosed() || pl_match->feature_from_id_ == 0) && "closed landmark with not full feature match");
+ assert((!pl_lmk->isClosed() || pl_match->feature_to_id_ == pl_ftr->getNPoints()-1) && "closed landmark with not full feature match");
}
// ESTABLISH CONSTRAINTS ------------------------------------------------------------------------
@@ -764,31 +762,37 @@ LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const LandmarkPolyline2DP
LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
{
+ LandmarkMatchPolyline2DPtr lmk_match = nullptr;
+
// compute best sq distance matching
- LandmarkMatchPolyline2DPtr match_pl = computeBestSqDist(_lmk_expected_feature_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,
- params_->position_error_th_*params_->position_error_th_);
- // still valid match
- if (match_pl)
+ MatchPolyline2DPtr match = computeBestSqDist(_lmk_expected_feature_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_->position_error_th_*params_->position_error_th_);
+ // valid match
+ if (match)
{
- match_pl->landmark_ptr_=_lmk_ptr;
- match_pl->landmark_version_=_lmk_ptr->getVersion();
+ lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
+ 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();
- match_pl->landmark_from_id_ = lmk_valid_ids[match_pl->landmark_from_id_];
- match_pl->landmark_to_id_ = lmk_valid_ids[match_pl->landmark_to_id_];
-
- assert(match_pl->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
- assert(match_pl->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
+ 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_;
+
+ assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
+ assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
}
- return match_pl;
+ return lmk_match;
}
bool ProcessorPolyline::rejectOutlier(ConstraintBasePtr& ctr_ptr)
diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline.cpp
index 480ebc7a339dcbe291cf8eb602efbac42bf0ed85..12522a5780a737ee8e610d077a7428dc85c056e3 100644
--- a/src/processor/processor_tracker_feature_polyline.cpp
+++ b/src/processor/processor_tracker_feature_polyline.cpp
@@ -44,10 +44,10 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(ftr_last_);
// TODO: move _features_last_in according to motion
- FeatureMatchPolyline2DPtr best_pair_match = computeBestMatch(pl_last,pl_incoming);
+ FeatureMatchPolyline2DPtr best_pair_match = tryMatchWithFeature(pl_last,pl_incoming);
// best match
- best_ftr_match = bestMatch(best_ftr_match, best_pair_match);
+ best_ftr_match = bestFeatureMatch(best_ftr_match, best_pair_match);
// TODO: undo move _features_last_in according to motion
}
@@ -354,12 +354,38 @@ void ProcessorTrackerFeaturePolyline::postProcess()
}
-FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::computeBestMatch(const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I) const
+FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I) const
{
- /* 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]
- */
+ FeatureMatchPolyline2DPtr ftr_match = nullptr;
+
+ // compute best sq distance matching
+ MatchPolyline2DPtr match = computeBestSqDist(_ftr_L->getPoints(), // <--feature last points
+ _ftr_L->isFirstDefined(),
+ _ftr_L->isLastDefined(),
+ false, // <--feature is not closed for sure
+ _ftr_I->getPoints(), // <--feature incoming points
+ _ftr_I->isFirstDefined(),
+ _ftr_I->isLastDefined(),
+ false, // <--feature is not closed for sure
+ params_tracker_feature_polyline_->position_error_th_*params_tracker_feature_polyline_->position_error_th_);
+ // valid match
+ if (match)
+ {
+ ftr_match = std::make_shared<FeatureMatchPolyline2D>();
+ // feature last point id's
+ ftr_match->feature_last_from_id_ = match->from_A_id_;
+ ftr_match->feature_last_to_id_ = match->to_A_id_;
+ // feature incoming point id's
+ ftr_match->feature_incoming_from_id_ = match->from_B_id_;
+ ftr_match->feature_incoming_to_id_ = match->to_B_id_;
+ }
+
+ return ftr_match;
+
+ /*
+ //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;
@@ -479,7 +505,7 @@ FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::computeBestMatch(cons
current_match->normalized_score_ = dist2.mean();
// Choose the most overlapped one
- best_match = bestMatch(best_match,current_match);
+ best_match = bestFeatureMatch(best_match,current_match);
if (print && best_match == current_match)
std::cout << "\tBEST MATCH" << std::endl;
@@ -487,9 +513,10 @@ FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::computeBestMatch(cons
}
return best_match;
+ */
}
-wolf::FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::bestMatch(const FeatureMatchPolyline2DPtr& _match_A, const FeatureMatchPolyline2DPtr& _match_B) const
+wolf::FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A, const FeatureMatchPolyline2DPtr& _match_B) const
{
assert(_match_A != nullptr || _match_B != nullptr);
if (_match_A == nullptr)
@@ -498,9 +525,11 @@ wolf::FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::bestMatch(const
if (_match_B == nullptr)
return _match_A;
- if (_match_A->feature_incoming_to_id_ - _match_A->feature_incoming_from_id_
- >= _match_B->feature_incoming_to_id_ - _match_B->feature_incoming_from_id_
- && _match_A->normalized_score_ < _match_B->normalized_score_)
+ // 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;
@@ -519,31 +548,37 @@ LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::tryMatchWithLandmark
LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
{
+ LandmarkMatchPolyline2DPtr lmk_match = nullptr;
+
// compute best sq distance matching
- LandmarkMatchPolyline2DPtr match_pl = computeBestSqDist(_lmk_expected_feature_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,
- params_tracker_feature_polyline_->match_position_error_th*params_tracker_feature_polyline_->match_position_error_th);
- // still valid match
- if (match_pl)
+ MatchPolyline2DPtr match = computeBestSqDist(_lmk_expected_feature_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_->position_error_th_*params_tracker_feature_polyline_->position_error_th_);
+ // valid match
+ if (match)
{
- match_pl->landmark_ptr_=_lmk_ptr;
- match_pl->landmark_version_=_lmk_ptr->getVersion();
+ lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
+ 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();
- match_pl->landmark_from_id_ = lmk_valid_ids[match_pl->landmark_from_id_];
- match_pl->landmark_to_id_ = lmk_valid_ids[match_pl->landmark_to_id_];
-
- assert(match_pl->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
- assert(match_pl->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
+ 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_;
+
+ assert(lmk_match->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
+ assert(lmk_match->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
}
- return match_pl;
+ return lmk_match;
}
// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt
index 484793812a146123926cfcc6388184c8b822743a..0050eaff1960eaf2aa8fbc11878732def14dbc03 100644
--- a/test/CMakeLists.txt
+++ b/test/CMakeLists.txt
@@ -180,10 +180,15 @@ target_link_libraries(gtest_landmark_polyline ${PROJECT_NAME})
wolf_add_gtest(gtest_make_posdef gtest_make_posdef.cpp)
target_link_libraries(gtest_make_posdef ${PROJECT_NAME})
+# Polyline 2D test
+wolf_add_gtest(gtest_polyline_2D gtest_polyline_2D.cpp)
+target_link_libraries(gtest_polyline_2D ${PROJECT_NAME})
+
# Parameter prior test
wolf_add_gtest(gtest_param_prior gtest_param_prior.cpp)
target_link_libraries(gtest_param_prior ${PROJECT_NAME})
+
# Pinhole test
wolf_add_gtest(gtest_pinhole gtest_pinhole.cpp)
target_link_libraries(gtest_pinhole ${PROJECT_NAME})
diff --git a/test/gtest_polyline_2D.cpp b/test/gtest_polyline_2D.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..7bbe9f189300ede9f769f6ed33c9c6e771bcc64f
--- /dev/null
+++ b/test/gtest_polyline_2D.cpp
@@ -0,0 +1,45 @@
+#include <stdlib.h>
+#include "utils_gtest.h"
+#include "base/processor/polyline_2D_utils.h"
+
+using namespace wolf;
+using namespace Eigen;
+
+TEST(Polyline2DTest, RigidTransformation)
+{
+ // Random points around random position
+ MatrixXs points_last = MatrixXs::Random(3,8);
+ points_last.bottomRows(1) = MatrixXs::Ones(1,8);
+ MatrixXs T_random = transMatrix2D(Vector2s::Random(), (rand() % 2000*M_PI)*0.001);
+ points_last = T_random * points_last;
+
+ // For different orientation changes between last and incomming
+ for (Scalar theta_last_incoming = -M_PI; theta_last_incoming < M_PI; theta_last_incoming += 0.1*M_PI)
+ {
+ // movement from last to incoming (random translation)
+ Matrix2s R_last_incoming = Rotation2D<Scalar>(theta_last_incoming).matrix();
+ Vector2s t_last_incoming(Vector2s::Random());
+
+ // inverse transformation
+ Scalar theta_incoming_last = -theta_last_incoming;
+ Vector2s t_incoming_last = -R_last_incoming.transpose() * t_last_incoming;
+ Matrix3s T_incoming_last = transMatrix2D(t_incoming_last, theta_incoming_last);
+
+ // rotate points
+ MatrixXs points_incoming = T_incoming_last * points_last;
+
+ // compute rigit transformation
+ Vector3s v = computeRigidTrans(points_incoming, points_last);
+
+ ASSERT_MATRIX_APPROX(v.head(2), t_incoming_last, 1e-12);
+ ASSERT_DOUBLE_EQ(v(2), theta_incoming_last);
+ }
+
+// PRINTF("All good at TestTest::DummyTestExample !\n");
+}
+
+int main(int argc, char **argv)
+{
+ testing::InitGoogleTest(&argc, argv);
+ return RUN_ALL_TESTS();
+}