diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8e7d93a2b99530226cdacaa478464f8ff6389a34..447618a490642e374e5745e5164470456f626bf8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -379,6 +379,7 @@ SET(HDRS_FEATURE
SET(HDRS_LANDMARK
include/base/landmark/landmark_base.h
include/base/landmark/landmark_match.h
+ include/base/landmark/landmark_match_polyline_2D.h
include/base/landmark/landmark_corner_2D.h
include/base/landmark/landmark_container.h
include/base/landmark/landmark_line_2D.h
diff --git a/include/base/feature/feature_match_polyline_2D.h b/include/base/feature/feature_match_polyline_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..38624b6f3c346ab581cce34662c20d51732dbcc6
--- /dev/null
+++ b/include/base/feature/feature_match_polyline_2D.h
@@ -0,0 +1,30 @@
+#ifndef FEATURE_MATCH_POLYLINE_2D_H_
+#define FEATURE_MATCH_POLYLINE_2D_H_
+
+// Wolf includes
+#include "base/wolf.h"
+#include "base/feature/feature_match.h"
+
+//wolf nampseace
+namespace wolf {
+
+//forward declaration to typedef class pointers
+WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatchPolyline2D);
+
+/** \brief Match between a feature_polyline_2D and a feature_polyline_2D
+ *
+ * Match between a feature_polyline_2D and a feature_polyline_2D (feature-feature correspondence)
+ *
+ */
+struct FeatureMatchPolyline2D : public FeatureMatch
+{
+ int feature_last_from_id_;
+ int feature_last_to_id_;
+ int feature_incoming_from_id_;
+ int feature_incoming_to_id_;
+};
+
+}//end namespace
+
+#endif
+
diff --git a/include/base/landmark/landmark_match_polyline_2D.h b/include/base/landmark/landmark_match_polyline_2D.h
new file mode 100644
index 0000000000000000000000000000000000000000..3298c99645a3d0d157795e49adb52c85f72a1f1e
--- /dev/null
+++ b/include/base/landmark/landmark_match_polyline_2D.h
@@ -0,0 +1,31 @@
+#ifndef LANDMARK_MATCH_POLYLINE_2D_H_
+#define LANDMARK_MATCH_POLYLINE_2D_H_
+
+// Wolf includes
+#include "base/wolf.h"
+#include "base/landmark/landmark_match.h"
+
+//wolf nampseace
+namespace wolf {
+
+// Map of Feature - Landmark matches
+WOLF_STRUCT_PTR_TYPEDEFS(LandmarkMatchPolyline2D);
+typedef std::map<FeatureBasePtr, LandmarkMatchPolyline2DPtr> LandmarkMatchPolyline2DMap;
+
+/** \brief Match between a feature_polyline_2D and a landmark_polyline_2D
+ *
+ * Match between a feature_polyline_2D and a landmark_polyline_2D
+ *
+ **/
+struct LandmarkMatchPolyline2D : public LandmarkMatch
+{
+ int landmark_from_id_;
+ int feature_from_id_;
+ int landmark_to_id_;
+ int feature_to_id_;
+ int landmark_version_;
+};
+
+}//end namespace
+
+#endif
diff --git a/include/base/landmark/landmark_polyline_2D.h b/include/base/landmark/landmark_polyline_2D.h
index 13db3f53acd6aaf8cd13f9293a21312bc1e9c7be..62624e9cac7fcaa474dd731452c38141e97690ce 100644
--- a/include/base/landmark/landmark_polyline_2D.h
+++ b/include/base/landmark/landmark_polyline_2D.h
@@ -10,6 +10,7 @@
// Wolf
#include "base/landmark/landmark_base.h"
+#include "base/processor/polyline_2D_utils.h"
// STL
#include <deque>
@@ -211,9 +212,7 @@ class LandmarkPolyline2D : public LandmarkBase
YAML::Node saveToYaml() const;
- /** \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) const;
+ static void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_tol);
};
inline StateBlockPtr& LandmarkPolyline2D::firstStateBlock()
@@ -253,6 +252,9 @@ inline bool LandmarkPolyline2D::isClosed() const
inline LandmarkPolyline2DPtr LandmarkPolyline2D::getMergedInLandmark() const
{
+ // recursive call
+ if (merged_in_lmk_->getMergedInLandmark() != nullptr)
+ return merged_in_lmk_->getMergedInLandmark();
return merged_in_lmk_;
}
diff --git a/include/base/processor/polyline_2D_utils.h b/include/base/processor/polyline_2D_utils.h
new file mode 100644
index 0000000000000000000000000000000000000000..947db7d7028739e0f9df6c73469d1b0f99204b31
--- /dev/null
+++ b/include/base/processor/polyline_2D_utils.h
@@ -0,0 +1,357 @@
+/*
+ * polyline_2D_utils.h
+ *
+ * Created on: Mar 14, 2019
+ * Author: jvallve
+ */
+
+#ifndef POLYLINE_2D_UTILS_H_
+#define POLYLINE_2D_UTILS_H_
+
+#include "base/wolf.h"
+#include "base/landmark/landmark_match_polyline_2D.h"
+#include "base/landmark/landmark_polyline_2D.h"
+#include "base/feature/feature_match_polyline_2D.h"
+#include "base/feature/feature_polyline_2D.h"
+
+namespace wolf {
+
+/** \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=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;
+
+ 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
+ // 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);
+}
+
+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
+
+#endif /* POLYLINE_2D_UTILS_H_ */
diff --git a/include/base/processor/processor_polyline.h b/include/base/processor/processor_polyline.h
index ea13e37babf77294c3c8cc74fcf09cddee34002b..a9f05d94c223e84c8e5784c8a08b9b93fc9e5e08 100644
--- a/include/base/processor/processor_polyline.h
+++ b/include/base/processor/processor_polyline.h
@@ -27,47 +27,9 @@
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;
-
-WOLF_STRUCT_PTR_TYPEDEFS(LandmarkPolylineMatch);
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsPolyline);
WOLF_PTR_TYPEDEFS(ProcessorPolyline);
-// 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_;
- int landmark_version_;
-
- LandmarkPolylineMatch() :
- landmark_match_from_id_(0),
- feature_match_from_id_(0),
- landmark_match_to_id_(0),
- feature_match_to_id_(0),
- landmark_version_(0)
- {
- //
- }
- LandmarkPolylineMatch(int _landmark_match_from_id,
- int _feature_match_from_id,
- int _landmark_match_to_id,
- int _feature_match_to_id,
- int _landmark_version) :
- 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),
- landmark_version_(_landmark_version)
- {
- //
- }
-};
-
struct ProcessorParamsPolyline : public ProcessorParamsBase
{
laserscanutils::LineFinderIterativeParams line_finder_params;
@@ -76,15 +38,10 @@ struct ProcessorParamsPolyline : public ProcessorParamsBase
unsigned int new_features_th;
Scalar loop_time_th;
std::vector<PolylineRectangularClass> polyline_classes;
-
- // 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;
+ Scalar position_error_th_;
+ Scalar min_features_ratio_th_;
};
-
class ProcessorPolyline : public ProcessorBase
{
protected:
@@ -146,11 +103,8 @@ class ProcessorPolyline : public ProcessorBase
const int& _lmk_point_id);
bool rejectOutlier(ConstraintBasePtr& ctr_ptr);
void classifyPolylineLandmarks(LandmarkBaseList& _lmk_list);
- void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list) const;
- LandmarkPolylineMatchPtr 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, bool print=false) const;
- LandmarkPolylineMatchPtr tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr);
- LandmarkPolylineMatchPtr tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
+ LandmarkMatchPolyline2DPtr tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr);
+ LandmarkMatchPolyline2DPtr tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
// Gets/Sets
const FeatureBaseList& getLastNewPolylines() const;
@@ -162,16 +116,6 @@ class ProcessorPolyline : public ProcessorBase
void computeTransformations(const TimeStamp& _ts);
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, bool strict = false) const;
-
- // The orthogonal projection of B over the line A-Aaux is in the segment [A,Aaux].
- bool isProjectedOverSegment(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux, const Eigen::Vector3s& _B);
// Factory method
public:
diff --git a/include/base/processor/processor_tracker_feature_polyline.h b/include/base/processor/processor_tracker_feature_polyline.h
index a2823077ef8d883d0c37310dd4e0551a93641375..dbc701e3bef4257f9e05e537e64a0c0edc4e2500 100644
--- a/include/base/processor/processor_tracker_feature_polyline.h
+++ b/include/base/processor/processor_tracker_feature_polyline.h
@@ -8,12 +8,14 @@
#ifndef PROCESSOR_TRACKER_FEATURE_POLYLINE_H_
#define PROCESSOR_TRACKER_FEATURE_POLYLINE_H_
+#include "base/processor/polyline_2D_utils.h"
#include "base/processor/processor_tracker_feature.h"
-#include "base/landmark/landmark_match.h"
#include "base/landmark/landmark_polyline_2D.h"
+#include "base/landmark/landmark_match_polyline_2D.h"
#include "base/sensor/sensor_laser_2D.h"
#include "base/capture/capture_laser_2D.h"
#include "base/feature/feature_polyline_2D.h"
+#include "base/feature/feature_match_polyline_2D.h"
#include "base/constraint/constraint_point_2D.h"
#include "base/constraint/constraint_point_to_line_2D.h"
@@ -26,7 +28,7 @@ namespace wolf
{
WOLF_STRUCT_PTR_TYPEDEFS(ProcessorParamsTrackerFeaturePolyline);
-WOLF_STRUCT_PTR_TYPEDEFS(FeaturePolylineMatch);
+WOLF_STRUCT_PTR_TYPEDEFS(FeatureMatchPolyline2D);
WOLF_PTR_TYPEDEFS(ProcessorTrackerFeaturePolyline);
struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeature
@@ -45,27 +47,6 @@ struct ProcessorParamsTrackerFeaturePolyline : public ProcessorParamsTrackerFeat
Scalar min_features_ratio_th_;// = 0.5;
};
-// Match Feature - Feature
-struct FeaturePolylineMatch : public FeatureMatch
-{
- int feature_last_from_id_;
- int feature_last_to_id_;
- int feature_incoming_from_id_;
- int feature_incoming_to_id_;
-};
-
-// Match Feature - Landmark
-WOLF_STRUCT_PTR_TYPEDEFS(LandmarkPolylineMatch);
-typedef std::map<FeatureBasePtr, LandmarkPolylineMatchPtr> LandmarkPolylineMatchMap;
-struct LandmarkPolylineMatch : public LandmarkMatch
-{
- int landmark_from_id_;
- int feature_from_id_;
- int landmark_to_id_;
- int feature_to_id_;
- int landmark_version_;
-};
-
class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
{
protected:
@@ -73,7 +54,7 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
laserscanutils::LineFinderIterative line_finder_;
FeatureBaseList all_features_incoming_, all_features_last_;
- LandmarkPolylineMatchMap landmark_match_map_;
+ LandmarkMatchPolyline2DMap landmark_match_map_;
Eigen::Matrix2s R_sensor_world_last_, R_world_sensor_last_;
Eigen::Vector2s t_sensor_world_last_, t_world_sensor_last_;
@@ -90,6 +71,8 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
virtual ~ProcessorTrackerFeaturePolyline();
+ virtual void configure(SensorBasePtr _sensor){};
+
protected:
/** \brief Track provided features from \b last to \b incoming
* \param _features_last_in input list of features in \b last to track
@@ -183,16 +166,19 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
*/
virtual void postProcess() override;
- FeaturePolylineMatchPtr computeBestMatch(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
+ FeatureMatchPolyline2DPtr computeBestMatch(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
- FeaturePolylineMatchConstPtr bestMatch(const FeaturePolylineMatchConstPtr& _match_A,
- const FeaturePolylineMatchConstPtr& _match_B) const;
+ FeatureMatchPolyline2DPtr bestMatch(const FeatureMatchPolyline2DPtr& _match_A,
+ const FeatureMatchPolyline2DPtr& _match_B) const;
- Scalar sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict) const
+ LandmarkMatchPolyline2DPtr tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
+
+ LandmarkMatchPolyline2DPtr tryMatchWithLandmark(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
void computeIncomingTransformations(const TimeStamp& _ts);
+ void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const;
+
public:
/// @brief Factory method
diff --git a/src/landmark/landmark_polyline_2D.cpp b/src/landmark/landmark_polyline_2D.cpp
index e64510750858b82bd6ed04fc6ece3dce6c13ff60..3694b28eb19b735285fe5a17a60eeabcb42b80a3 100644
--- a/src/landmark/landmark_polyline_2D.cpp
+++ b/src/landmark/landmark_polyline_2D.cpp
@@ -421,36 +421,6 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
return false;
}
-Scalar LandmarkPolyline2D::sqDistPoint2Segment(const Eigen::Vector2s& p, const Eigen::Vector2s& a, const Eigen::Vector2s& b) const
-{
- Scalar ap_sq = (p-a).squaredNorm();
- Scalar bp_sq = (p-b).squaredNorm();
- Scalar ab_sq = (b-a).squaredNorm();
- Scalar sqDist;
- //WOLF_DEBUG("a = ", a);
- //WOLF_DEBUG("b = ", b);
- //WOLF_DEBUG("p = ", p);
- //WOLF_DEBUG("ap_sq = ", ap_sq);
- //WOLF_DEBUG("bp_sq = ", bp_sq);
- //WOLF_DEBUG("ab_sq = ", ab_sq);
-
- // If the projection of p onto ab is inside the segment (acute triangle: all angles < 90º), return the sq distance point-line.
- // Otherwise, return the sq distance to the closest segment point.
- if (ap_sq <= bp_sq + ab_sq && bp_sq <= ap_sq + ab_sq) // acute triangle: applying pitagoras twice, angles ABP and PAB <= 90º
- {
- //WOLF_DEBUG("projection of P onto AB -> returning point-line");
- sqDist = ap_sq - std::pow((b-a).dot(p-a),2) / ab_sq;
- }
- else
- {
- //WOLF_DEBUG("projection of P out of AB -> returning min point-point");
- sqDist = std::min(bp_sq,ap_sq);
- }
-
- //WOLF_DEBUG("sq_dist = ", sqDist);
- return sqDist;
-}
-
void LandmarkPolyline2D::setClosed()
{
assert(getNDefinedPoints() >= 2 && "closing a polyline with less than 2 defined points");
@@ -997,6 +967,79 @@ YAML::Node LandmarkPolyline2D::saveToYaml() const
return node;
}
+void tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list, const Scalar& _dist_tol)
+{
+ std::cout << "LandmarkPolyline2D::tryMergeLandmarks\n";
+ assert(_lmk_list.size() >= 2);
+
+ // 2 by 2
+ LandmarkPolyline2DPtr remaining_lmk = _lmk_list.front();
+ _lmk_list.pop_front();
+ LandmarkPolyline2DPtr A_lmk, B_lmk, 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_list.pop_front();
+
+ if (B_lmk->isRemoving())
+ continue;
+
+ std::cout << "Candidates to be merged: " << A_lmk->id() << " " << B_lmk->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);
+
+ // match
+ if ( match )
+ {
+ std::vector<int> valid_id_A = A_lmk->getValidIds();
+ std::vector<int> valid_id_B = B_lmk->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()))
+ {
+ 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_);
+ }
+ 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_);
+ }
+
+ // merge 2 landmarks
+ remaining_lmk->mergeLandmark(merged_lmk, remaining_from, remaining_to, merged_from, merged_to);
+
+ // reset pointers
+ A_lmk.reset();
+ B_lmk.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);
+ }
+ std::cout << "tryMergeLandmarks::done\n";
+}
+
// Register landmark creator
namespace
{
diff --git a/src/processor/processor_polyline.cpp b/src/processor/processor_polyline.cpp
index 77345600678116afc24d37e8088d6926e2bd915b..890f7eccb812e599ccc09746d962a09ac3bd0fb8 100644
--- a/src/processor/processor_polyline.cpp
+++ b/src/processor/processor_polyline.cpp
@@ -207,7 +207,7 @@ void ProcessorPolyline::process(CaptureBasePtr _incoming_ptr)
continue;
// Merge landmarks candidates and accumulate the correspondence of merged to the remaining ones
- tryMergeLandmarks(merge_candidates);
+ LandmarkPolyline2D::tryMergeLandmarks(merge_candidates, params_->position_error_th_);
}
//WOLF_DEBUG( "-----End of post-process():");
}
@@ -292,7 +292,7 @@ unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks
}
// NAIVE NEAREST NEIGHBOR MATCHING
- LandmarkPolylineMatchPtr best_match = nullptr;
+ LandmarkMatchPolyline2DPtr best_match = nullptr;
FeaturePolyline2DPtr pl_ftr;
LandmarkPolyline2DPtr pl_lmk;
LandmarkPolyline2DList merging_candidates;
@@ -315,12 +315,12 @@ unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks
if (print)
std::cout << "\tcompute Best Sq Dist of landmark " << pl_lmk->id() << " (" << pl_lmk->getNPoints()
<< ") and feature " << pl_ftr->id() << "(" << pl_ftr->getNPoints() << ")\n";
- LandmarkPolylineMatchPtr best_correspondence = tryMatch(expected_features[pl_lmk],pl_lmk,pl_ftr);
+ LandmarkMatchPolyline2DPtr best_correspondence = tryMatch(expected_features[pl_lmk],pl_lmk,pl_ftr);
// best match
if (best_correspondence != nullptr &&
(best_match == nullptr ||
- (best_correspondence->feature_match_to_id_-best_correspondence->feature_match_from_id_ >= best_match->feature_match_to_id_-best_match->feature_match_from_id_ &&
+ (best_correspondence->feature_to_id_-best_correspondence->feature_from_id_ >= best_match->feature_to_id_-best_match->feature_from_id_ &&
best_correspondence->normalized_score_ < best_match->normalized_score_ ) ) )
{
if (print)
@@ -345,8 +345,8 @@ unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks
// make a copy of current iterator
auto it_found = std::prev(feature_it);
- assert(best_match->feature_match_from_id_ == 0 || !std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_)->isClosed());
- assert(best_match->feature_match_to_id_ == std::static_pointer_cast<FeaturePolyline2D>(*it_found)->getNPoints()-1 || !std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_)->isClosed());
+ assert(best_match->feature_from_id_ == 0 || !std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_)->isClosed());
+ assert(best_match->feature_to_id_ == std::static_pointer_cast<FeaturePolyline2D>(*it_found)->getNPoints()-1 || !std::static_pointer_cast<LandmarkPolyline2D>(best_match->landmark_ptr_)->isClosed());
bool print = false;//best_match->landmark_ptr_->id() == 13;
if (print)
@@ -354,10 +354,10 @@ unsigned int ProcessorPolyline::findLandmarks(const LandmarkBaseList& _landmarks
std::cout << "\tlandmark : " << best_match->landmark_ptr_->id() << std::endl;
std::cout << "\tlandmark version: " << best_match->landmark_version_ << std::endl;
std::cout << "\tfeature : " << (*it_found)->id() << std::endl;
- std::cout << "\tlandmark from id: " << best_match->landmark_match_from_id_ << std::endl;
- std::cout << "\tlandmark to id : " << best_match->landmark_match_to_id_ << std::endl;
- std::cout << "\tfeature from id : " << best_match->feature_match_from_id_ << std::endl;
- std::cout << "\tfeature to id : " << best_match->feature_match_to_id_ << std::endl;
+ std::cout << "\tlandmark from id: " << best_match->landmark_from_id_ << std::endl;
+ std::cout << "\tlandmark to id : " << best_match->landmark_to_id_ << std::endl;
+ std::cout << "\tfeature from id : " << best_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeature to id : " << best_match->feature_to_id_ << std::endl;
}
// match
_feature_landmark_correspondences[*it_found] = best_match;
@@ -393,11 +393,11 @@ void ProcessorPolyline::createNewLandmarks()
// 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;
+ LandmarkMatchPolyline2DPtr match = std::make_shared<LandmarkMatchPolyline2D>();
+ match->feature_from_id_= 0; // all points match
+ match->landmark_from_id_ = 0;
+ match->feature_to_id_= polyline_ft_ptr->getNPoints()-1; // all points match
+ match->landmark_to_id_ = polyline_ft_ptr->getNPoints()-1;
match->normalized_score_ = 1.0; // max score
match->landmark_ptr_ = new_lmk_ptr;
match->landmark_version_ = 0;
@@ -409,10 +409,10 @@ void ProcessorPolyline::createNewLandmarks()
new_landmarks_.push_back(new_lmk_ptr);
// std::cout << "new match: feature " << new_feature_ptr->id() << " with landmark " << new_lmk_ptr->id() << std::endl;
- // std::cout << "\tfeature_match_from_id_ = " << match->feature_match_from_id_ << std::endl;
- // std::cout << "\tlandmark_match_from_id_ = " << match->landmark_match_from_id_ << std::endl;
- // std::cout << "\tfeature_match_to_id_ = " << match->feature_match_to_id_ << std::endl;
- // std::cout << "\tlandmark_match_to_id_ = " << match->landmark_match_to_id_ << std::endl;
+ // std::cout << "\tfeature_from_id_ = " << match->feature_from_id_ << std::endl;
+ // std::cout << "\tlandmark_from_id_ = " << match->landmark_from_id_ << std::endl;
+ // std::cout << "\tfeature_to_id_ = " << match->feature_to_id_ << std::endl;
+ // std::cout << "\tlandmark_to_id_ = " << match->landmark_to_id_ << std::endl;
// std::cout << "\tnormalized_score_ = " << match->normalized_score_ << std::endl;
}
//std::cout << "done" << std::endl;
@@ -446,7 +446,7 @@ void ProcessorPolyline::establishConstraints()
//std::cout << "ProcessorPolyline::establishConstraints" << std::endl;
//std::cout << "\tlast_ id: " << last_ptr_->id() << " linked to frame: " << last_ptr_->getFramePtr()->id() << " is linked to problem? " << (last_ptr_->getProblem() ? "YES" : "NO") << std::endl;
unsigned int N_constraints = 0;
- LandmarkPolylineMatchPtr pl_match;
+ LandmarkMatchPolyline2DPtr pl_match;
FeaturePolyline2DPtr pl_ftr;
LandmarkPolyline2DPtr pl_lmk;
LandmarkPolyline2DList modified_lmks;
@@ -454,10 +454,10 @@ void ProcessorPolyline::establishConstraints()
for (auto last_ftr : last_ptr_->getFeatureList())
{
pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(last_ftr);
- pl_match = std::static_pointer_cast<LandmarkPolylineMatch>(matches_landmark_from_last_[last_ftr]);
+ pl_match = std::static_pointer_cast<LandmarkMatchPolyline2D>(matches_landmark_from_last_[last_ftr]);
pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(pl_match->landmark_ptr_);
- assert(pl_match->feature_match_from_id_ >= 0 && pl_match->feature_match_to_id_ >= 0);
+ assert(pl_match->feature_from_id_ >= 0 && pl_match->feature_to_id_ >= 0);
assert(pl_lmk != nullptr && pl_match != nullptr);
if (pl_lmk->isRemoving())
@@ -469,11 +469,10 @@ void ProcessorPolyline::establishConstraints()
if (pl_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
{
// get the not merged lmk
- while (pl_lmk->getMergedInLandmark() != nullptr)
- pl_lmk = pl_lmk->getMergedInLandmark();
+ pl_lmk = pl_lmk->getMergedInLandmark();
// redo matching
- LandmarkPolylineMatchPtr updated_match = tryMatch(pl_lmk, pl_ftr);
+ LandmarkMatchPolyline2DPtr updated_match = tryMatch(pl_lmk, pl_ftr);
// if still matching, overwrite match
if (updated_match)
@@ -486,7 +485,7 @@ void ProcessorPolyline::establishConstraints()
continue;
}
- assert(pl_match->feature_match_from_id_ >= 0 && pl_match->feature_match_to_id_ >= 0);
+ assert(pl_match->feature_from_id_ >= 0 && pl_match->feature_to_id_ >= 0);
}
// MODIFY LANDMARK (only for not closed and not recently created)
@@ -503,10 +502,10 @@ void ProcessorPolyline::establishConstraints()
std::cout << "\tpoints " << pl_lmk->getNPoints() << std::endl;
std::cout << "\tfirst defined " << pl_lmk->isFirstDefined() << std::endl;
std::cout << "\tlast defined " << pl_lmk->isLastDefined() << std::endl << std::endl;
- std::cout << "\tmatch from feature point " << pl_match->feature_match_from_id_ << std::endl;
- std::cout << "\tmatch to feature point " << pl_match->feature_match_to_id_ << std::endl;
- std::cout << "\tmatch from landmark point " << pl_match->landmark_match_from_id_ << std::endl;
- std::cout << "\tmatch to landmark point " << pl_match->landmark_match_to_id_ << std::endl;
+ std::cout << "\tmatch from feature point " << pl_match->feature_from_id_ << std::endl;
+ std::cout << "\tmatch to feature point " << pl_match->feature_to_id_ << std::endl;
+ std::cout << "\tmatch from landmark point " << pl_match->landmark_from_id_ << std::endl;
+ std::cout << "\tmatch to landmark point " << pl_match->landmark_to_id_ << std::endl;
}
Eigen::MatrixXs points_global = R_world_sensor_ * pl_ftr->getPoints().topRows<2>() +
t_world_sensor_ * Eigen::VectorXs::Ones(pl_ftr->getNPoints()).transpose();
@@ -514,36 +513,36 @@ void ProcessorPolyline::establishConstraints()
// MODIFY LANDMARK
// -----------------GROW Front-----------------
// Add new front points (if any feature point not matched)
- if ( pl_match->feature_match_from_id_ > 0 ) // && !pl_lmk->isClosed()
+ if ( pl_match->feature_from_id_ > 0 ) // && !pl_lmk->isClosed()
{
assert(!pl_lmk->isClosed() && "feature has not matched points in a closed landmark");
if (print)
{
std::cout << "Add new front points. Defined: " << pl_ftr->isFirstDefined() << std::endl;
- std::cout << "\tfeat from " << pl_match->feature_match_from_id_ << std::endl;
- std::cout << "\tfeat to " << pl_match->feature_match_to_id_ << std::endl;
- std::cout << "\tland from " << pl_match->landmark_match_from_id_ << std::endl;
- std::cout << "\tland to " << pl_match->landmark_match_to_id_ << std::endl;
+ std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
+ std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
+ std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
}
pl_lmk->addPoints(points_global, // points matrix in global coordinates
- pl_match->feature_match_from_id_-1, // last feature point index to be added
+ pl_match->feature_from_id_-1, // last feature point index to be added
pl_ftr->isFirstDefined(), // is defined
false); // front
modified_lmks.push_back(pl_lmk);
- pl_match->landmark_match_from_id_ = pl_lmk->getFirstId();
- pl_match->feature_match_from_id_ = 0;
- //std::cout << "\tfeat from " << pl_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << pl_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << pl_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << pl_match->landmark_match_to_id_ << std::endl;
+ pl_match->landmark_from_id_ = pl_lmk->getFirstId();
+ pl_match->feature_from_id_ = 0;
+ //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
+ //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
+ //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
+ //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
}
// -----------------CHANGE Front-----------------
// Change first not defined point if landmark first not defined matched with first feature point that:
// a. is defined
// b. would extend the line (check if angle is bigger than 90º)
- else if (pl_match->landmark_match_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_match_from_id_ == 0
+ else if (pl_match->landmark_from_id_ == pl_lmk->getFirstId() && !pl_lmk->isFirstDefined()) // && pl_match->feature_from_id_ == 0
{
if (print)
{
@@ -563,39 +562,39 @@ void ProcessorPolyline::establishConstraints()
modified_lmks.push_back(pl_lmk);
}
}
- assert(pl_match->feature_match_from_id_ == 0 && "Landmark didn't grow properly");
+ assert(pl_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
// -----------------GROW Back-----------------
// Add new back points (if any feature point not matched)
- if (pl_match->feature_match_to_id_ < pl_ftr->getNPoints()-1)
+ if (pl_match->feature_to_id_ < pl_ftr->getNPoints()-1)
{
assert(!pl_lmk->isClosed() && "feature not matched points in a closed landmark");
if (print)
{
std::cout << "Add back points. Defined: " << pl_ftr->isLastDefined() << std::endl;
- std::cout << "\tfeat from " << pl_match->feature_match_from_id_ << std::endl;
- std::cout << "\tfeat to " << pl_match->feature_match_to_id_ << std::endl;
- std::cout << "\tland from " << pl_match->landmark_match_from_id_ << std::endl;
- std::cout << "\tland to " << pl_match->landmark_match_to_id_ << std::endl;
+ std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
+ std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
+ std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
+ std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
}
pl_lmk->addPoints(points_global, // points matrix in global coordinates
- pl_match->feature_match_to_id_+1, // last feature point index to be added
+ pl_match->feature_to_id_+1, // last feature point index to be added
pl_ftr->isLastDefined(), // is defined
true); // back
modified_lmks.push_back(pl_lmk);
- pl_match->landmark_match_to_id_ = pl_lmk->getLastId();
- pl_match->feature_match_to_id_ = pl_ftr->getNPoints()-1;
- //std::cout << "\tfeat from " << pl_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tfeat to " << pl_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tland from " << pl_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tland to " << pl_match->landmark_match_to_id_ << std::endl;
+ pl_match->landmark_to_id_ = pl_lmk->getLastId();
+ pl_match->feature_to_id_ = pl_ftr->getNPoints()-1;
+ //std::cout << "\tfeat from " << pl_match->feature_from_id_ << std::endl;
+ //std::cout << "\tfeat to " << pl_match->feature_to_id_ << std::endl;
+ //std::cout << "\tland from " << pl_match->landmark_from_id_ << std::endl;
+ //std::cout << "\tland to " << pl_match->landmark_to_id_ << std::endl;
}
// -----------------CHANGE Back-----------------
// Change last not defined point if landmark last not defined matched with last feature point that:
// a. is defined
// b. would extend the line (check if angle is bigger than 90º)
- else if (pl_match->landmark_match_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_match_to_id_ == pl_ftr->getNPoints()-1
+ else if (pl_match->landmark_to_id_ == pl_lmk->getLastId() && !pl_lmk->isLastDefined()) //&& pl_match->feature_to_id_ == pl_ftr->getNPoints()-1
{
if (print)
{
@@ -615,7 +614,7 @@ void ProcessorPolyline::establishConstraints()
modified_lmks.push_back(pl_lmk);
}
}
- assert(pl_match->feature_match_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+ assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
if (print)
{
@@ -636,30 +635,30 @@ void ProcessorPolyline::establishConstraints()
{
// add recently created landmarks to the list of modified landmark
modified_lmks.push_back(pl_lmk);
- assert(pl_match->feature_match_from_id_ == 0 && "recently created landmark with not full feature match");
- assert(pl_match->feature_match_to_id_ == pl_ftr->getNPoints()-1 && "recently created landmark with not full feature match");
+ 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_match_from_id_ == 0 && "closed landmark with not full feature match");
- assert(pl_match->feature_match_to_id_ == pl_ftr->getNPoints()-1 && "closed landmark with not full feature match");
+ 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");
}
}
// ESTABLISH CONSTRAINTS ------------------------------------------------------------------------
- assert(pl_match->feature_match_from_id_ == 0 && "Landmark didn't grow properly");
- assert(pl_match->feature_match_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
- assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + pl_match->landmark_match_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly");
+ assert(pl_match->feature_from_id_ == 0 && "Landmark didn't grow properly");
+ assert(pl_match->feature_to_id_ == pl_ftr->getNPoints()-1 && "Landmark didn't grow properly");
+ assert((pl_lmk->getNPoints() >= pl_ftr->getNPoints() + pl_match->landmark_from_id_ || pl_lmk->isClosed()) && "Landmark didn't grow properly");
//std::cout << "Establishing constraint between" << std::endl;
//std::cout << "\tfeature " << pl_ftr->id() << " is liked to capture? " << (pl_ftr->getCapturePtr() ? pl_ftr->getCapturePtr()->id() : 9999999999999999) << " is liked to problem? " << (pl_ftr->getProblem() ? "YES" : "NO") << std::endl;
//std::cout << "\tlandmark " << pl_lmk->id() << std::endl;
- //std::cout << "\tmatch from feature point " << pl_match->feature_match_from_id_ << std::endl;
- //std::cout << "\tmatch to feature point " << pl_match->feature_match_to_id_ << std::endl;
- //std::cout << "\tmatch from landmark point " << pl_match->landmark_match_from_id_ << std::endl;
- //std::cout << "\tmatch to landmark point " << pl_match->landmark_match_to_id_ << std::endl;
+ //std::cout << "\tmatch from feature point " << pl_match->feature_from_id_ << std::endl;
+ //std::cout << "\tmatch to feature point " << pl_match->feature_to_id_ << std::endl;
+ //std::cout << "\tmatch from landmark point " << pl_match->landmark_from_id_ << std::endl;
+ //std::cout << "\tmatch to landmark point " << pl_match->landmark_to_id_ << std::endl;
// Constraints for all feature points
- int lmk_point_id = pl_match->landmark_match_from_id_;
+ int lmk_point_id = pl_match->landmark_from_id_;
for (int ftr_point_id = 0; ftr_point_id < pl_ftr->getNPoints(); ftr_point_id++)
{
@@ -751,305 +750,9 @@ void ProcessorPolyline::classifyPolylineLandmarks(LandmarkBaseList& _lmk_list)
//std::cout << "ProcessorPolyline::classifyPolylineLandmarks: done\n";
}
-void ProcessorPolyline::tryMergeLandmarks(LandmarkPolyline2DList& _lmk_list) const
-{
- std::cout << "ProcessorPolyline::tryMergeLandmarks\n";
- assert(_lmk_list.size() >= 2);
-
- // 2 by 2
- LandmarkPolyline2DPtr remaining_lmk = _lmk_list.front();
- _lmk_list.pop_front();
- LandmarkPolyline2DPtr A_lmk, B_lmk, 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_list.pop_front();
-
- if (B_lmk->isRemoving())
- continue;
-
- std::cout << "Candidates to be merged: " << A_lmk->id() << " " << B_lmk->id() << "\n";
-
- // if any closed, it should be A
- if (!A_lmk->isClosed() && B_lmk->isClosed())
- {
- A_lmk = B_lmk;
- B_lmk = remaining_lmk;
- }
-
- // check best correspondence
- Eigen::MatrixXs points_A = A_lmk->computePointsGlobal();
- Eigen::MatrixXs points_B = B_lmk->computePointsGlobal();
- LandmarkPolylineMatchPtr match = computeBestSqDist(points_A, A_lmk->isFirstDefined(), A_lmk->isLastDefined(), A_lmk->isClosed(),
- points_B, B_lmk->isFirstDefined(), B_lmk->isLastDefined(), B_lmk->isClosed(), false);
-
- // match
- if ( match )
- {
- std::vector<int> valid_id_A = A_lmk->getValidIds();
- std::vector<int> valid_id_B = B_lmk->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()))
- {
- remaining_lmk = A_lmk;
- merged_lmk = B_lmk;
- remaining_from = valid_id_A.at(match->landmark_match_from_id_);
- remaining_to = valid_id_A.at(match->landmark_match_to_id_);
- merged_from = valid_id_B.at(match->feature_match_from_id_);
- merged_to = valid_id_B.at(match->feature_match_to_id_);
- }
- else
- {
- remaining_lmk = B_lmk;
- merged_lmk = A_lmk;
- remaining_from = valid_id_B.at(match->feature_match_from_id_);
- remaining_to = valid_id_B.at(match->feature_match_to_id_);
- merged_from = valid_id_A.at(match->landmark_match_from_id_);
- merged_to = valid_id_A.at(match->landmark_match_to_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();
- assert(merged_lmk->getHits() == 0);
- assert(std::find(getProblem()->getMapPtr()->getLandmarkList().begin(), getProblem()->getMapPtr()->getLandmarkList().end(), merged_lmk) == getProblem()->getMapPtr()->getLandmarkList().end());
- std::cout << "merged\n";
- }
- // no match (remaining: most constraints)
- else
- remaining_lmk = (A_lmk->getHits() > B_lmk->getHits() ? A_lmk : B_lmk);
- }
- std::cout << "tryMergeLandmarks::done\n";
-}
-LandmarkPolylineMatchPtr ProcessorPolyline::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,
- bool print) const
-{
- if (print)
- {
- std::cout << "ProcessorPolyline::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
- */
-
- LandmarkPolylineMatchPtr best_match = nullptr;
-
- // ----------------- Call switching A<->B
- if ( (!_closed_A && !_closed_B && _points_A.cols() < _points_B.cols()) || // (a)
- (!_closed_A && _closed_B) ) // (b)
- {
- if (print)
- std::cout << "Auto-calling switching A<->B...\n";
-
- LandmarkPolylineMatchPtr best_match = computeBestSqDist(_points_B, _first_defined_B, _last_defined_B, _closed_B,
- _points_A, _first_defined_A, _last_defined_A, _closed_A, print);
- // undo switching
- if (best_match != nullptr)
- {
- std::swap(best_match->feature_match_from_id_, best_match->landmark_match_from_id_);
- std::swap(best_match->feature_match_to_id_, best_match->landmark_match_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;
-
- 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();
-
- if (print)
- {
- 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);
- if (print)
- {
- 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);
- if (print)
- 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);
- if (print)
- 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);
- }
- if (print)
- std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl;
-
- // All squared distances should be within a threshold
- // Choose the most overlapped one
- if ((dist2 < params_->match_position_error_th*params_->match_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_ )))
- {
- if (print)
- std::cout << "\tBEST MATCH" << std::endl;
- if (best_match == nullptr)
- best_match = std::make_shared<LandmarkPolylineMatch>();
-
- best_match->feature_match_from_id_= from_B;
- best_match->feature_match_to_id_= to_B;
- best_match->landmark_ptr_= nullptr;
- best_match->normalized_score_ = dist2.mean();
- best_match->landmark_match_from_id_= from_A;
- best_match->landmark_match_to_id_= to_A;
- }
- }
-
- return best_match;
-}
-
-LandmarkPolylineMatchPtr ProcessorPolyline::tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr)
+LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr)
{
// updated expected feature
Eigen::MatrixXs exp_feat, exp_cov;
@@ -1059,17 +762,18 @@ LandmarkPolylineMatchPtr ProcessorPolyline::tryMatch(const LandmarkPolyline2DPtr
return tryMatch(exp_feat,_lmk_ptr, _feat_ptr);
}
-LandmarkPolylineMatchPtr ProcessorPolyline::tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
+LandmarkMatchPolyline2DPtr ProcessorPolyline::tryMatch(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
{
// compute best sq distance matching
- LandmarkPolylineMatchPtr match_pl = computeBestSqDist(_lmk_expected_feature_points, // <--landmark points in local coordinates
+ 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);
+ false,
+ params_->position_error_th_*params_->position_error_th_);
// still valid match
if (match_pl)
{
@@ -1077,11 +781,11 @@ LandmarkPolylineMatchPtr ProcessorPolyline::tryMatch(const Eigen::MatrixXs& _lmk
match_pl->landmark_version_=_lmk_ptr->getVersion();
// landmark point id's
std::vector<int> lmk_valid_ids = _lmk_ptr->getValidIds();
- match_pl->landmark_match_from_id_ = lmk_valid_ids[match_pl->landmark_match_from_id_];
- match_pl->landmark_match_to_id_ = lmk_valid_ids[match_pl->landmark_match_to_id_];
+ 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_match_from_id_ == 0 || !_lmk_ptr->isClosed());
- assert(match_pl->feature_match_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
+ assert(match_pl->feature_from_id_ == 0 || !_lmk_ptr->isClosed());
+ assert(match_pl->feature_to_id_ == _feat_ptr->getNPoints()-1 || !_lmk_ptr->isClosed());
}
return match_pl;
@@ -1197,170 +901,7 @@ void ProcessorPolyline::expectedFeature(LandmarkBasePtr _landmark_ptr, Eigen::Ma
expected_ftr_ = pl_lmk->computePointsGlobal();
expected_ftr_.topRows(2) = R_sensor_world_ * (expected_ftr_.topRows(2).colwise() - t_world_sensor_);
expected_ftr_cov_ = pl_lmk->computePointsCovGlobal();
-
- /*Eigen::MatrixXs expected_ftr2 = Eigen::MatrixXs::Zero(3,pl_lmk->getNPoints());
- Eigen::MatrixXs expected_ftr_cov2 = Eigen::MatrixXs::Zero(2,2*pl_lmk->getNPoints());
- Eigen::Vector3s col = Eigen::Vector3s::Ones();
-
- std::vector<int> valid_ids = pl_lmk->getValidIds();
-
- ////////// global coordinates points
- if (pl_lmk->getClassification().type == UNCLASSIFIED)
- for (int i = 0; i < pl_lmk->getNPoints(); i++)
- {
- //std::cout << "i " << i << " valid id: " << valid_ids[i] << std::endl;
- //std::cout << "Landmark global position: " << pl_lmk->getPointVector(valid_ids[i]).transpose() << std::endl;
-
- // ------------ expected feature point
- col.head<2>() = R_sensor_world_ * (pl_lmk->getPointVector(valid_ids[i]) - t_world_sensor_);
- expected_ftr2.col(i) = col;
-
- // std::cout << "Expected point " << i << ": " << expected_ftr_.col(i).transpose() << std::endl;
- // ------------ expected feature point covariance
- // TODO
- expected_ftr_cov2.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
- }
-
- ////////// landmark with origin
- else
- {
- Eigen::Matrix2s R_world_points = Eigen::Rotation2Ds(pl_lmk->getOPtr()->getState()(0)).matrix();
- const Eigen::VectorXs& t_world_points = pl_lmk->getPPtr()->getState();
-
- for (int i = 0; i < pl_lmk->getNPoints(); i++)
- {
- //std::cout << "i " << i << " valid id: " << valid_ids[i] << std::endl;
- //std::cout << "Landmark global position: " << pl_lmk->getPointVector(valid_ids[i]).transpose() << std::endl;
-
- // ------------ expected feature point
- col.head<2>() = R_sensor_world_ * (R_world_points * pl_lmk->getPointVector(valid_ids[i]) + t_world_points - t_world_sensor_);
- expected_ftr2.col(i) = col;
-
- // std::cout << "Expected point " << i << ": " << expected_ftr_.col(i).transpose() << std::endl;
- // ------------ expected feature point covariance
- // TODO
- expected_ftr_cov2.middleCols(i*2, 2) = Eigen::MatrixXs::Identity(2,2);
- }
- }
-
- if (!expected_ftr2.isApprox(expected_ftr_, 1e-6) || !expected_ftr_cov2.isApprox(expected_ftr_cov_, 1e-6))
- {
- WOLF_ERROR("ProcessorPolylines: expected features not properly computed...");
- std::cout << expected_ftr_ << std::endl;
- std::cout << expected_ftr2 << std::endl;
- }*/
-}
-
-Eigen::VectorXs ProcessorPolyline::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 ProcessorPolyline::sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict) const
-{
- /* 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;
-}
-
-bool ProcessorPolyline::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);
+ // TODO Rotate covariances
}
ProcessorBasePtr ProcessorPolyline::create(const std::string& _unique_name, const ProcessorParamsBasePtr _params, const SensorBasePtr)
diff --git a/src/processor/processor_tracker_feature_polyline.cpp b/src/processor/processor_tracker_feature_polyline.cpp
index adaba8e6b8eb42d992fbb8b48aae41ebfd4a90d1..480ebc7a339dcbe291cf8eb602efbac42bf0ed85 100644
--- a/src/processor/processor_tracker_feature_polyline.cpp
+++ b/src/processor/processor_tracker_feature_polyline.cpp
@@ -35,7 +35,7 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
// iterate over all polylines features
while (ftr_it != all_features_incoming_.end())
{
- FeaturePolylineMatchPtr best_ftr_match = nullptr;
+ FeatureMatchPolyline2DPtr best_ftr_match = nullptr;
auto pl_incoming = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
// Check matching with all features in last
@@ -44,7 +44,7 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
auto pl_last = std::static_pointer_cast<FeaturePolyline2D>(ftr_last_);
// TODO: move _features_last_in according to motion
- FeaturePolylineMatchPtr best_pair_match = computeBestMatch(pl_last,pl_incoming);
+ FeatureMatchPolyline2DPtr best_pair_match = computeBestMatch(pl_last,pl_incoming);
// best match
best_ftr_match = bestMatch(best_ftr_match, best_pair_match);
@@ -74,7 +74,7 @@ unsigned int ProcessorTrackerFeaturePolyline::trackFeatures(const FeatureBaseLis
pl_lmk->getMergedInLandmark() == nullptr &&
!pl_lmk->isRemoving())
{
- auto lmk_match_incoming = std::make_shared<LandmarkPolylineMatch>();
+ auto lmk_match_incoming = std::make_shared<LandmarkMatchPolyline2D>();
lmk_match_incoming->feature_from_id_ = best_ftr_match->feature_incoming_from_id_;
lmk_match_incoming->feature_to_id_ = best_ftr_match->feature_incoming_to_id_;
@@ -156,7 +156,7 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(*ftr_it);
// Search a match with existing landmarks
- LandmarkPolylineMatchPtr lmk_match = nullptr;
+ LandmarkMatchPolyline2DPtr lmk_match = nullptr;
for (auto lmk : getProblem()->getMapPtr()->getLandmarkList())
{
if (lmk->getType() != "POLYLINE 2D")
@@ -171,7 +171,7 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
auto new_lmk_ptr = createLandmark(pl_ftr);
// Add a new match to landmark_match_map_
- lmk_match = std::make_shared<LandmarkPolylineMatch>();
+ lmk_match = std::make_shared<LandmarkMatchPolyline2D>();
lmk_match->landmark_ptr_ = new_lmk_ptr;
lmk_match->landmark_version_ = std::static_pointer_cast<LandmarkPolyline2D>(new_lmk_ptr)->getVersion();
lmk_match->feature_from_id_= 0; // all points match
@@ -181,6 +181,9 @@ unsigned int ProcessorTrackerFeaturePolyline::processNew(const unsigned int& _ma
lmk_match->normalized_score_ = 1.0; // max score
landmark_match_map_[pl_ftr] = lmk_match;
+
+ // Add the new landmark to the map
+ getProblem()->addLandmark(new_lmk_ptr);
}
else
{
@@ -218,7 +221,7 @@ ConstraintBasePtr ProcessorTrackerFeaturePolyline::createConstraint(FeatureBaseP
{
assert(landmark_match_map_.find(_feature_ptr) != landmark_match_map_.end() && "feature without landmark match");
- auto match = landmark_match_map_[_feature_ptr];
+ auto match = landmark_match_map_.at(_feature_ptr);
// TODO
@@ -231,11 +234,25 @@ void ProcessorTrackerFeaturePolyline::establishConstraints()
for (auto ftr : last_ptr_->getFeatureList())
{
assert(landmark_match_map_.find(ftr) != landmark_match_map_.end() && "feature without landmark match in last features");
- auto lmk = landmark_match_map_[ftr]->landmark_ptr_;
+ auto pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(landmark_match_map_[ftr]->landmark_ptr_);
+ auto pl_ftr = std::static_pointer_cast<FeaturePolyline2D>(ftr);
+ auto lmk_match = landmark_match_map_[ftr];
+
+ // CHECK IF LANDMARK CHANGED
+ if (lmk_match->landmark_version_ != pl_lmk->getVersion() || pl_lmk->getMergedInLandmark() != nullptr)
+ {
+ // TODO
+ }
+
+ // MODIFY LANDMARK (only for not closed and not recently created)
+ if (!pl_lmk->isClosed() && pl_lmk->getHits() > 0)
+ {
+ // TODO
+ }
- auto ctr_ptr = createConstraint(ftr, lmk);
+ auto ctr_ptr = createConstraint(ftr, pl_lmk);
ftr->addConstraint(ctr_ptr);
- lmk->addConstrainedBy(ctr_ptr);
+ pl_lmk->addConstrainedBy(ctr_ptr);
}
}
@@ -245,7 +262,7 @@ LandmarkBasePtr ProcessorTrackerFeaturePolyline::createLandmark(FeatureBasePtr _
//std::cout << "Robot global pose: " << t_world_robot_.transpose() << std::endl;
//std::cout << "Sensor global pose: " << t_world_sensor_.transpose() << std::endl;
assert(_feature_ptr->getType() == "POLYLINE 2D");
- assert(std::find(_feature_ptr, new_features_last_.begin(),new_features_last_.end()) != new_features_last_.end() && "creating a landmark for a feature which is not in last capture");
+ assert(std::find(new_features_last_.begin(),new_features_last_.end(),_feature_ptr) != new_features_last_.end() && "creating a landmark for a feature which is not in last capture");
FeaturePolyline2DPtr polyline_ptr = std::static_pointer_cast<FeaturePolyline2D>(_feature_ptr);
@@ -337,7 +354,7 @@ void ProcessorTrackerFeaturePolyline::postProcess()
}
-FeaturePolylineMatchPtr ProcessorTrackerFeaturePolyline::computeBestMatch(const FeaturePolyline2DPtr& _ftr_L, const FeaturePolyline2DPtr& _ftr_I) const
+FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::computeBestMatch(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.
@@ -362,7 +379,7 @@ FeaturePolylineMatchPtr ProcessorTrackerFeaturePolyline::computeBestMatch(const
std::cout << "\tdefined points_I:\n" << points_I.topRows(2) << std::endl;
}
- FeaturePolylineMatchPtr best_match = nullptr;
+ FeatureMatchPolyline2DPtr best_match = nullptr;
// Check all overlapping configurations
int min_offset = -last_L+first_I;
@@ -453,7 +470,7 @@ FeaturePolylineMatchPtr ProcessorTrackerFeaturePolyline::computeBestMatch(const
// All squared distances should be within a threshold
if ((dist2 < params_tracker_feature_polyline_->match_position_error_th*params_tracker_feature_polyline_->match_position_error_th).all())
{
- FeaturePolylineMatchPtr current_match;
+ FeatureMatchPolyline2DPtr current_match;
current_match->feature_incoming_from_id_= from_I;
current_match->feature_incoming_to_id_= to_I;
current_match->feature_last_from_id_= from_L;
@@ -472,8 +489,7 @@ FeaturePolylineMatchPtr ProcessorTrackerFeaturePolyline::computeBestMatch(const
return best_match;
}
-wolf::FeaturePolylineMatchConstPtr ProcessorTrackerFeaturePolyline::bestMatch(
- const FeaturePolylineMatchConstPtr& _match_A, const FeaturePolylineMatchConstPtr& _match_B) const
+wolf::FeatureMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::bestMatch(const FeatureMatchPolyline2DPtr& _match_A, const FeatureMatchPolyline2DPtr& _match_B) const
{
assert(_match_A != nullptr || _match_B != nullptr);
if (_match_A == nullptr)
@@ -490,74 +506,62 @@ wolf::FeaturePolylineMatchConstPtr ProcessorTrackerFeaturePolyline::bestMatch(
return _match_B;
}
-Scalar ProcessorTrackerFeaturePolyline::sqDistPointToLine(const Eigen::Vector3s& _A, const Eigen::Vector3s& _A_aux,
- const Eigen::Vector3s& _B, bool _A_defined, bool _B_defined, bool strict) const
-{
- /* 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.");
+LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
+{
+ // updated expected feature
+ Eigen::MatrixXs exp_feat, exp_cov;
+ expectedFeatureLast(_lmk_ptr, exp_feat, exp_cov);
- 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();
+ // try match
+ return tryMatchWithLandmark(exp_feat,_lmk_ptr, _feat_ptr);
+}
- // squared distance to line
- // Case 1
- if (!_B_defined && !_A_defined)
+LandmarkMatchPolyline2DPtr ProcessorTrackerFeaturePolyline::tryMatchWithLandmark(const Eigen::MatrixXs& _lmk_expected_feature_points, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const
+{
+ // 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)
{
- 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);
+ match_pl->landmark_ptr_=_lmk_ptr;
+ match_pl->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());
}
- // 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));
- }
+ return match_pl;
+}
- // 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);
- }
+// MATH ///////////////////////////////////////////////////////////////////////////////////////////////////////////
+///////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+
+void ProcessorTrackerFeaturePolyline::expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const
+{
+ assert(_landmark_ptr->getType() == "POLYLINE 2D" && "ProcessorTrackerFeaturePolyline::expectedFeature: Bad landmark type");
+ LandmarkPolyline2DPtr pl_lmk = std::static_pointer_cast<LandmarkPolyline2D>(_landmark_ptr);
+
+ //std::cout << "ProcessorPolyline::expectedFeature" << std::endl;
+ //std::cout << "t_world_sensor_last_: " << t_world_sensor_last_.transpose() << std::endl;
+ //std::cout << "R_sensor_world_last_: " << std::endl << R_sensor_world_last_ << std::endl;
- // Default return unused
- return 1e12;
+ expected_feature_ = pl_lmk->computePointsGlobal();
+ expected_feature_.topRows(2) = R_sensor_world_last_ * (expected_feature_.topRows(2).colwise() - t_world_sensor_last_);
+ expected_feature_cov_ = pl_lmk->computePointsCovGlobal();
+ // TODO Rotate covariances
}
void ProcessorTrackerFeaturePolyline::computeIncomingTransformations(const TimeStamp& _ts)