Skip to content
Snippets Groups Projects
Commit 336ab3cf authored by Joan Solà Ortega's avatar Joan Solà Ortega
Browse files

Merge branch 'matchers' into 'master'

Matchers

Two "matchers" structs removed from wolf.h and declared in separate files. Update includes needing them accordingly.

See merge request !92
parents fe72f437 96659059
No related branches found
No related tags found
1 merge request!92Matchers
...@@ -160,39 +160,39 @@ TARGET_LINK_LIBRARIES(test_processor_imu_jacobians ${PROJECT_NAME}) ...@@ -160,39 +160,39 @@ TARGET_LINK_LIBRARIES(test_processor_imu_jacobians ${PROJECT_NAME})
# TARGET_LINK_LIBRARIES(test_capture_laser_2D ${PROJECT_NAME}) # TARGET_LINK_LIBRARIES(test_capture_laser_2D ${PROJECT_NAME})
# #ENDIF (laser_scan_utils_FOUND) # #ENDIF (laser_scan_utils_FOUND)
IF(faramotics_FOUND) # IF(faramotics_FOUND)
IF (laser_scan_utils_FOUND) # IF (laser_scan_utils_FOUND)
ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp) # ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
TARGET_LINK_LIBRARIES(test_ceres_2_lasers # TARGET_LINK_LIBRARIES(test_ceres_2_lasers
${pose_state_time_LIBRARIES} # ${pose_state_time_LIBRARIES}
${faramotics_LIBRARIES} # ${faramotics_LIBRARIES}
${PROJECT_NAME}) # ${PROJECT_NAME})
ADD_EXECUTABLE(test_ceres_2_lasers_polylines test_ceres_2_lasers_polylines.cpp) # ADD_EXECUTABLE(test_ceres_2_lasers_polylines test_ceres_2_lasers_polylines.cpp)
TARGET_LINK_LIBRARIES(test_ceres_2_lasers_polylines # TARGET_LINK_LIBRARIES(test_ceres_2_lasers_polylines
${pose_state_time_LIBRARIES} # ${pose_state_time_LIBRARIES}
${faramotics_LIBRARIES} # ${faramotics_LIBRARIES}
${PROJECT_NAME}) # ${PROJECT_NAME})
ADD_EXECUTABLE(test_2_lasers_offline test_2_lasers_offline.cpp) # ADD_EXECUTABLE(test_2_lasers_offline test_2_lasers_offline.cpp)
TARGET_LINK_LIBRARIES(test_2_lasers_offline # TARGET_LINK_LIBRARIES(test_2_lasers_offline
${pose_state_time_LIBRARIES} # ${pose_state_time_LIBRARIES}
${faramotics_LIBRARIES} # ${faramotics_LIBRARIES}
${PROJECT_NAME}) # ${PROJECT_NAME})
ADD_EXECUTABLE(test_faramotics_simulation test_faramotics_simulation.cpp) # ADD_EXECUTABLE(test_faramotics_simulation test_faramotics_simulation.cpp)
TARGET_LINK_LIBRARIES(test_faramotics_simulation # TARGET_LINK_LIBRARIES(test_faramotics_simulation
${pose_state_time_LIBRARIES} # ${pose_state_time_LIBRARIES}
${faramotics_LIBRARIES} # ${faramotics_LIBRARIES}
${PROJECT_NAME}) # ${PROJECT_NAME})
# ADD_EXECUTABLE(test_autodiff test_autodiff.cpp) # # ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
# TARGET_LINK_LIBRARIES(test_autodiff # # TARGET_LINK_LIBRARIES(test_autodiff
# ${pose_state_time_LIBRARIES} # # ${pose_state_time_LIBRARIES}
# ${faramotics_LIBRARIES} # # ${faramotics_LIBRARIES}
# ${PROJECT_NAME}) # # ${PROJECT_NAME})
# IF(Suitesparse_FOUND) # # IF(Suitesparse_FOUND)
# ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp) # # ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
# TARGET_LINK_LIBRARIES(test_iQR_wolf2 # # TARGET_LINK_LIBRARIES(test_iQR_wolf2
# ${pose_state_time_LIBRARIES} # # ${pose_state_time_LIBRARIES}
# ${faramotics_LIBRARIES} # # ${faramotics_LIBRARIES}
# ${PROJECT_NAME}) # # ${PROJECT_NAME})
# ENDIF(Suitesparse_FOUND) # # ENDIF(Suitesparse_FOUND)
ENDIF (laser_scan_utils_FOUND) # # ENDIF (laser_scan_utils_FOUND)
ENDIF(faramotics_FOUND) # # ENDIF(faramotics_FOUND)
#ifndef FEATURE_MATCH_H_
#define FEATURE_MATCH_H_
// Wolf includes
#include "wolf.h"
//wolf nampseace
namespace wolf {
/** \brief Match between a feature and a feature
*
* Match between a feature and a feature (feature-feature correspondence)
*
**/
struct FeatureMatch
{
FeatureBasePtr feature_ptr_;
Scalar normalized_score_;
};
typedef std::shared_ptr<FeatureMatch> FeatureMatchPtr;
typedef std::map<FeatureBasePtr, FeatureMatchPtr> FeatureMatchMap;
}//end namespace
#endif
#ifndef LANDMARK_MATCH_H_
#define LANDMARK_MATCH_H_
// Wolf includes
#include "wolf.h"
//wolf nampseace
namespace wolf {
/** \brief Match between a feature and a landmark
*
* Match between a feature and a landmark
*
**/
struct LandmarkMatch
{
LandmarkBasePtr landmark_ptr_;
Scalar normalized_score_;
LandmarkMatch() :
landmark_ptr_(nullptr), normalized_score_(0)
{
}
LandmarkMatch(LandmarkBasePtr _landmark_ptr, Scalar _normalized_score) :
landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
{
}
};
// Map of Feature - Landmark matches
typedef std::shared_ptr<LandmarkMatch> LandmarkMatchPtr;
typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
}//end namespace
#endif
...@@ -144,7 +144,7 @@ unsigned int ProcessorImageFeature::trackFeatures(const FeatureBaseList& _featur ...@@ -144,7 +144,7 @@ unsigned int ProcessorImageFeature::trackFeatures(const FeatureBaseList& _featur
incoming_point_ptr->setTrackId(feature_ptr->trackId()); incoming_point_ptr->setTrackId(feature_ptr->trackId());
_feature_matches[incoming_point_ptr] = FeatureMatch({feature_base_ptr, normalized_score}); _feature_matches[incoming_point_ptr] = std::make_shared<FeatureMatch>(FeatureMatch({feature_base_ptr, normalized_score}));
} }
else else
{ {
......
...@@ -39,8 +39,7 @@ unsigned int ProcessorTrackerFeature::processKnown() ...@@ -39,8 +39,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
auto known_incoming_feature_it = known_features_incoming_.begin(); auto known_incoming_feature_it = known_features_incoming_.begin();
while (known_incoming_feature_it != known_features_incoming_.end()) while (known_incoming_feature_it != known_features_incoming_.end())
{ {
if (!correctFeatureDrift(matches_origin_from_last_[matches_last_from_incoming_[*known_incoming_feature_it].feature_ptr_].feature_ptr_, if (!correctFeatureDrift(matches_origin_from_last_[matches_last_from_incoming_[*known_incoming_feature_it]->feature_ptr_]->feature_ptr_, matches_last_from_incoming_[*known_incoming_feature_it]->feature_ptr_,*known_incoming_feature_it))
matches_last_from_incoming_[*known_incoming_feature_it].feature_ptr_,*known_incoming_feature_it))
{ {
// Correspondence not confirmed -> Remove correspondence and destruct incoming feature // Correspondence not confirmed -> Remove correspondence and destruct incoming feature
matches_last_from_incoming_.erase(*known_incoming_feature_it); matches_last_from_incoming_.erase(*known_incoming_feature_it);
......
...@@ -8,8 +8,10 @@ ...@@ -8,8 +8,10 @@
#ifndef PROCESSOR_TRACKER_FEATURE_H_ #ifndef PROCESSOR_TRACKER_FEATURE_H_
#define PROCESSOR_TRACKER_FEATURE_H_ #define PROCESSOR_TRACKER_FEATURE_H_
//wolf includes
#include "processor_tracker.h" #include "processor_tracker.h"
#include "capture_base.h" #include "capture_base.h"
#include "feature_match.h"
#include "wolf.h" #include "wolf.h"
namespace wolf namespace wolf
...@@ -172,10 +174,10 @@ inline void ProcessorTrackerFeature::establishConstraints() ...@@ -172,10 +174,10 @@ inline void ProcessorTrackerFeature::establishConstraints()
{ {
for (auto match : matches_origin_from_last_) for (auto match : matches_origin_from_last_)
{ {
auto ctr = createConstraint(match.first, match.second.feature_ptr_); auto ctr = createConstraint(match.first, match.second->feature_ptr_);
ctr->setFeatureOtherPtr(match.second.feature_ptr_); ctr->setFeatureOtherPtr(match.second->feature_ptr_);
match.first->addConstraint(ctr); match.first->addConstraint(ctr);
match.second.feature_ptr_->addConstrainedBy(ctr); match.second->feature_ptr_->addConstrainedBy(ctr);
} }
} }
...@@ -187,7 +189,7 @@ inline void ProcessorTrackerFeature::advance() ...@@ -187,7 +189,7 @@ inline void ProcessorTrackerFeature::advance()
for (auto match : matches_last_from_incoming_) for (auto match : matches_last_from_incoming_)
{ {
matches_last_from_incoming_[match.first] = matches_last_from_incoming_[match.first] =
matches_origin_from_last_[matches_last_from_incoming_[match.first].feature_ptr_]; matches_origin_from_last_[matches_last_from_incoming_[match.first]->feature_ptr_];
} }
matches_origin_from_last_ = std::move(matches_last_from_incoming_); matches_origin_from_last_ = std::move(matches_last_from_incoming_);
......
...@@ -64,7 +64,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList& ...@@ -64,7 +64,7 @@ unsigned int ProcessorTrackerFeatureCorner::trackFeatures(const FeatureBaseList&
if (((*feat_out_it)->getMeasurement().head<3>() - expected_feature_pose).squaredNorm() > position_error_th_*position_error_th_) if (((*feat_out_it)->getMeasurement().head<3>() - expected_feature_pose).squaredNorm() > position_error_th_*position_error_th_)
{ {
// match // match
_feature_correspondences[*feat_out_it] = FeatureMatch({feat_in_ptr,0}); _feature_correspondences[*feat_out_it] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
// move matched feature to list // move matched feature to list
_feature_list_out.splice(_feature_list_out.end(), corners_incoming_, feat_out_it); _feature_list_out.splice(_feature_list_out.end(), corners_incoming_, feat_out_it);
......
...@@ -29,7 +29,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList& ...@@ -29,7 +29,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
else else
{ {
_feature_list_out.push_back(std::make_shared<FeatureBase>(FEATURE_POINT_IMAGE, "POINT IMAGE", feat_in_ptr->getMeasurement(), feat_in_ptr->getMeasurementCovariance())); _feature_list_out.push_back(std::make_shared<FeatureBase>(FEATURE_POINT_IMAGE, "POINT IMAGE", feat_in_ptr->getMeasurement(), feat_in_ptr->getMeasurementCovariance()));
_feature_correspondences[_feature_list_out.back()] = FeatureMatch({feat_in_ptr,0}); _feature_correspondences[_feature_list_out.back()] = std::make_shared<FeatureMatch>(FeatureMatch({feat_in_ptr,0}));
std::cout << "feature " << feat_in_ptr->getMeasurement() << " tracked!" << std::endl; std::cout << "feature " << feat_in_ptr->getMeasurement() << " tracked!" << std::endl;
} }
} }
......
...@@ -8,8 +8,10 @@ ...@@ -8,8 +8,10 @@
#ifndef PROCESSOR_TRACKER_LANDMARK_H_ #ifndef PROCESSOR_TRACKER_LANDMARK_H_
#define PROCESSOR_TRACKER_LANDMARK_H_ #define PROCESSOR_TRACKER_LANDMARK_H_
//wolf includes
#include "processor_tracker.h" #include "processor_tracker.h"
#include "capture_base.h" #include "capture_base.h"
#include "landmark_match.h"
namespace wolf namespace wolf
{ {
......
...@@ -378,36 +378,14 @@ typedef std::shared_ptr<StateQuaternion> StateQuaternionPtr; ...@@ -378,36 +378,14 @@ typedef std::shared_ptr<StateQuaternion> StateQuaternionPtr;
// - - Local Parametrization // - - Local Parametrization
typedef std::shared_ptr<LocalParametrizationBase> LocalParametrizationBasePtr; typedef std::shared_ptr<LocalParametrizationBase> LocalParametrizationBasePtr;
// - - Match Feature - Landmark // // Feature-Feature correspondence
struct LandmarkMatch // struct FeatureMatch
{ // {
LandmarkBasePtr landmark_ptr_; // FeatureBasePtr feature_ptr_;
Scalar normalized_score_; // Scalar normalized_score_;
LandmarkMatch() : // };
landmark_ptr_(nullptr), normalized_score_(0) //
{ // typedef std::map<FeatureBasePtr, FeatureMatch> FeatureMatchMap;
}
LandmarkMatch(LandmarkBasePtr _landmark_ptr, Scalar _normalized_score) :
landmark_ptr_(_landmark_ptr), normalized_score_(_normalized_score)
{
}
};
// Match map Feature - Landmark
typedef std::shared_ptr<LandmarkMatch> LandmarkMatchPtr;
typedef std::map<FeatureBasePtr, LandmarkMatchPtr> LandmarkMatchMap;
// Feature-Feature correspondence
struct FeatureMatch
{
FeatureBasePtr feature_ptr_;
Scalar normalized_score_;
};
typedef std::map<FeatureBasePtr, FeatureMatch> FeatureMatchMap;
......
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