Skip to content
Snippets Groups Projects
Commit 773e02ba authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

debugging

parent 050341d7
No related branches found
No related tags found
No related merge requests found
...@@ -372,6 +372,7 @@ SET(HDRS_FEATURE ...@@ -372,6 +372,7 @@ SET(HDRS_FEATURE
include/base/feature/feature_polyline_2D.h include/base/feature/feature_polyline_2D.h
include/base/feature/feature_base.h include/base/feature/feature_base.h
include/base/feature/feature_match.h include/base/feature/feature_match.h
include/base/feature/feature_match_polyline_2D.h
include/base/feature/feature_pose.h include/base/feature/feature_pose.h
include/base/feature/feature_gnss_fix.h include/base/feature/feature_gnss_fix.h
include/base/feature/feature_gnss_single_diff.h include/base/feature/feature_gnss_single_diff.h
...@@ -626,6 +627,9 @@ IF (laser_scan_utils_FOUND) ...@@ -626,6 +627,9 @@ IF (laser_scan_utils_FOUND)
SET(HDRS_SENSOR ${HDRS_SENSOR} SET(HDRS_SENSOR ${HDRS_SENSOR}
include/base/sensor/sensor_laser_2D.h include/base/sensor/sensor_laser_2D.h
) )
SET(SRCS_CAPTURE ${SRCS_CAPTURE}
src/capture/capture_laser_2D.cpp
)
SET(SRCS_PROCESSOR ${SRCS_PROCESSOR} SET(SRCS_PROCESSOR ${SRCS_PROCESSOR}
src/processor/processor_polyline.cpp src/processor/processor_polyline.cpp
src/processor/processor_tracker_feature_corner.cpp src/processor/processor_tracker_feature_corner.cpp
......
...@@ -253,7 +253,7 @@ inline bool LandmarkPolyline2D::isClosed() const ...@@ -253,7 +253,7 @@ inline bool LandmarkPolyline2D::isClosed() const
inline LandmarkPolyline2DPtr LandmarkPolyline2D::getMergedInLandmark() const inline LandmarkPolyline2DPtr LandmarkPolyline2D::getMergedInLandmark() const
{ {
// recursive call // recursive call
if (merged_in_lmk_->getMergedInLandmark() != nullptr) if (merged_in_lmk_ != nullptr && merged_in_lmk_->getMergedInLandmark() != nullptr)
return merged_in_lmk_->getMergedInLandmark(); return merged_in_lmk_->getMergedInLandmark();
return merged_in_lmk_; return merged_in_lmk_;
} }
......
...@@ -171,27 +171,25 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature ...@@ -171,27 +171,25 @@ class ProcessorTrackerFeaturePolyline : public ProcessorTrackerFeature
*/ */
virtual void postProcess() override; virtual void postProcess() override;
FeatureMatchPolyline2DList tryMatchWithFeature(const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming) const;
void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming, const Eigen::Matrix3s& _T_last_incoming_prior) const; void tryMatchWithFeature(FeatureMatchPolyline2DScalarMap& ftr_matches, const FeaturePolyline2DPtr& _ftr_last, const FeaturePolyline2DPtr& _ftr_incoming, const Eigen::Matrix3s& _T_last_incoming_prior) const;
// FeatureMatchPolyline2DPtr bestFeatureMatch(const FeatureMatchPolyline2DPtr& _match_A,
// const FeatureMatchPolyline2DPtr& _match_B) const;
LandmarkMatchPolyline2DList tryMatchWithLandmark(const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr) const;
void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const; void tryMatchWithLandmark(LandmarkMatchPolyline2DScalarMap& lmk_matches, const LandmarkPolyline2DPtr& _lmk_ptr, const FeaturePolyline2DPtr& _feat_ptr, const Eigen::Matrix3s& _T_feature_landmark_prior) const;
bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr); bool updateMatchWithLandmark(LandmarkMatchPolyline2DPtr& _lmk_match, LandmarkPolyline2DPtr& _lmk_ptr, FeaturePolyline2DPtr& _feat_ptr);
void computeTransformations(); void computeTransformations();
//void expectedFeatureLast(LandmarkBasePtr _landmark_ptr, Eigen::MatrixXs& expected_feature_, Eigen::MatrixXs& expected_feature_cov_) const;
public: public:
/// @brief Factory method /// @brief Factory method
static ProcessorBasePtr create(const std::string& _unique_name, static ProcessorBasePtr create(const std::string& _unique_name,
const ProcessorParamsBasePtr _params, const ProcessorParamsBasePtr _params,
const SensorBasePtr _sensor_ptr); const SensorBasePtr _sensor_ptr);
const FeatureBaseList& getLastNewFeatures() const
{
return all_features_last_;
}
}; };
} /* namespace wolf */ } /* namespace wolf */
......
...@@ -26,7 +26,7 @@ Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T) ...@@ -26,7 +26,7 @@ Eigen::Vector3s T2pose2D(const Eigen::Matrix3s& T)
{ {
Eigen::Vector3s pose; Eigen::Vector3s pose;
pose.head(2) = T.topRightCorner(2,1); pose.head(2) = T.topRightCorner(2,1);
pose(2) = atan2(T(0,0),T(1,0)); pose(2) = atan2(T(1,0),T(0,0));
return pose; return pose;
} }
...@@ -36,6 +36,7 @@ Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen: ...@@ -36,6 +36,7 @@ Eigen::Vector3s computeRigidTrans(const Eigen::MatrixXs& _points_A, const Eigen:
assert(_points_A.cols() == _points_B.cols()); assert(_points_A.cols() == _points_B.cols());
assert(_points_A.rows() >= 2); assert(_points_A.rows() >= 2);
assert(_points_B.rows() >= 2); assert(_points_B.rows() >= 2);
assert(_points_A.cols() >= 2);
Eigen::Vector3s t_A_B; Eigen::Vector3s t_A_B;
...@@ -362,8 +363,8 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool ...@@ -362,8 +363,8 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
const Scalar& max_sq_error ) const Scalar& max_sq_error )
{ {
//std::cout << "computeBestRigidTrans...\n"; //std::cout << "computeBestRigidTrans...\n";
//std::cout << "\tA is " << (_closed_A ? "CLOSED" : "OPEN") << " has "<< _points_A.cols() << " points\n"; //std::cout << "\tA is " << (_closed_A ? "CLOSED" : "NOT CLOSED") << " has "<< _points_A.cols() << " points\n";
//std::cout << "\tB is " << (_closed_B ? "CLOSED" : "OPEN") << " has "<< _points_B.cols() << " points\n"; //std::cout << "\tB is " << (_closed_B ? "CLOSED" : "NOT CLOSED") << " has "<< _points_B.cols() << " points\n";
//std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl; //std::cout << "\tpoints_A:\n" << _points_A.topRows(2) << std::endl;
//std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl; //std::cout << "\tpoints_B:\n" << _points_B.topRows(2) << std::endl;
...@@ -393,7 +394,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool ...@@ -393,7 +394,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
//std::cout << "Auto-calling switching A<->B...\n"; //std::cout << "Auto-calling switching A<->B...\n";
matches = computeBestRigidTrans(_points_B, _first_defined_B, _last_defined_B, _closed_B, matches = computeBestRigidTrans(_points_B, _first_defined_B, _last_defined_B, _closed_B,
_points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error); _points_A, _first_defined_A, _last_defined_A, _closed_A, max_sq_error);
// undo switching // undo switching
for (auto match : matches) for (auto match : matches)
{ {
...@@ -472,8 +473,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool ...@@ -472,8 +473,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
//std::cout << "\t\tto_A_defined " << to_A_defined << (to_A == 0) << (to_A == last_A) << _last_defined_A << std::endl; //std::cout << "\t\tto_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; //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 only one overlapped point, no rigid transformation possible
if ( N_overlapped== 1 && if ( N_overlapped == 1 )
continue;
// If only two overlapped points, all should be defined
if ( N_overlapped == 2 &&
(!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) ) (!from_A_defined || !from_B_defined || !to_A_defined || !to_B_defined ) )
continue; continue;
...@@ -489,6 +493,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool ...@@ -489,6 +493,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
assert(to_B < _points_B.cols()); assert(to_B < _points_B.cols());
// RIGID TRANSFORMATION FOR DEFINED POINTS // RIGID TRANSFORMATION FOR DEFINED POINTS
//std::cout << "\tcomputing rigid transformation...\n";
int from_B_def = from_B_defined ? from_B : from_B+1; int from_B_def = from_B_defined ? from_B : from_B+1;
int from_A_def = from_A_defined ? from_A : from_A+1; int from_A_def = from_A_defined ? from_A : from_A+1;
int to_A_def = to_A_defined ? to_A : to_A+1; int to_A_def = to_A_defined ? to_A : to_A+1;
...@@ -502,7 +507,9 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool ...@@ -502,7 +507,9 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
points_A_def.leftCols(_points_A.cols()-from_A_def) = _points_A.middleCols(from_A_def, _points_A.cols()-from_A_def); points_A_def.leftCols(_points_A.cols()-from_A_def) = _points_A.middleCols(from_A_def, _points_A.cols()-from_A_def);
points_A_def.rightCols(to_A_def+1) = _points_A.middleCols(0, to_A_def+1); points_A_def.rightCols(to_A_def+1) = _points_A.middleCols(0, to_A_def+1);
} }
//std::cout << "\tpoints filled...\n";
//std::cout << "\tcomputeRigidTrans\n";
Eigen::Vector3s pose_A_B = computeRigidTrans(points_A_def, points_B_def); Eigen::Vector3s pose_A_B = computeRigidTrans(points_A_def, points_B_def);
Eigen::Matrix3s T_A_B = pose2T2D(pose_A_B.head(2), pose_A_B(2)); Eigen::Matrix3s T_A_B = pose2T2D(pose_A_B.head(2), pose_A_B(2));
...@@ -512,6 +519,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool ...@@ -512,6 +519,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
dist2.middleRows(from_B_defined && from_A_defined ? 0 : 1, N_overlapped_def) = d.colwise().squaredNorm().transpose().array(); dist2.middleRows(from_B_defined && from_A_defined ? 0 : 1, N_overlapped_def) = d.colwise().squaredNorm().transpose().array();
// POINT-LINE DISTANCES for not defined extremes // POINT-LINE DISTANCES for not defined extremes
//std::cout << "\tPOINT-LINE DISTANCES\n";
// First // First
if (!from_B_defined || !from_A_defined) if (!from_B_defined || !from_A_defined)
{ {
...@@ -546,7 +554,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool ...@@ -546,7 +554,7 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
to_B_defined); to_B_defined);
assert(N_overlapped > 1); assert(N_overlapped > 1);
} }
//std::cout << "\t\tsquared distances = " << dist2.transpose() << std::endl; //std::cout << "\t\tsquared distances = " << dist2.transpose() << "( should be < " << max_sq_error << ")" << std::endl;
// All squared distances should be within a threshold // All squared distances should be within a threshold
if ((dist2 < max_sq_error).all()) if ((dist2 < max_sq_error).all())
...@@ -562,9 +570,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool ...@@ -562,9 +570,11 @@ MatchPolyline2DList computeBestRigidTrans(const Eigen::MatrixXs& _points_A, bool
// Store matches ordered by the squared norm of the transformation (I know, mixing mts and rads..) // Store matches ordered by the squared norm of the transformation (I know, mixing mts and rads..)
matches.push_back(match); matches.push_back(match);
std::cout << "match added to list of matches!\n";
} }
} }
std::cout << matches.size() << " rigid transformations found\n";
return matches; return matches;
} }
......
This diff is collapsed.
...@@ -5,6 +5,13 @@ ...@@ -5,6 +5,13 @@
using namespace wolf; using namespace wolf;
using namespace Eigen; using namespace Eigen;
TEST(Polyline2DTest, Transformations)
{
ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector2s::Zero(),0), 1e-12);
ASSERT_MATRIX_APPROX(Matrix3s::Identity(), pose2T2D(Vector3s::Zero()), 1e-12);
ASSERT_MATRIX_APPROX(Vector3s::Zero(), T2pose2D(Matrix3s::Identity()), 1e-12);
}
TEST(Polyline2DTest, RigidTransformation) TEST(Polyline2DTest, RigidTransformation)
{ {
// Random points around random position // Random points around random position
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment