diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index 7de8d464b45e2d9e1d6ec7209d48cfda81fa9af6..f70e214918bc345e096eb00d2947c9f5002e53a6 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -160,39 +160,39 @@ TARGET_LINK_LIBRARIES(test_processor_imu_jacobians ${PROJECT_NAME})
# TARGET_LINK_LIBRARIES(test_capture_laser_2D ${PROJECT_NAME})
# #ENDIF (laser_scan_utils_FOUND)
-IF(faramotics_FOUND)
- IF (laser_scan_utils_FOUND)
- ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
- TARGET_LINK_LIBRARIES(test_ceres_2_lasers
- ${pose_state_time_LIBRARIES}
- ${faramotics_LIBRARIES}
- ${PROJECT_NAME})
- ADD_EXECUTABLE(test_ceres_2_lasers_polylines test_ceres_2_lasers_polylines.cpp)
- TARGET_LINK_LIBRARIES(test_ceres_2_lasers_polylines
- ${pose_state_time_LIBRARIES}
- ${faramotics_LIBRARIES}
- ${PROJECT_NAME})
- ADD_EXECUTABLE(test_2_lasers_offline test_2_lasers_offline.cpp)
- TARGET_LINK_LIBRARIES(test_2_lasers_offline
- ${pose_state_time_LIBRARIES}
- ${faramotics_LIBRARIES}
- ${PROJECT_NAME})
- ADD_EXECUTABLE(test_faramotics_simulation test_faramotics_simulation.cpp)
- TARGET_LINK_LIBRARIES(test_faramotics_simulation
- ${pose_state_time_LIBRARIES}
- ${faramotics_LIBRARIES}
- ${PROJECT_NAME})
-# ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
-# TARGET_LINK_LIBRARIES(test_autodiff
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# IF(Suitesparse_FOUND)
-# ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
-# TARGET_LINK_LIBRARIES(test_iQR_wolf2
-# ${pose_state_time_LIBRARIES}
-# ${faramotics_LIBRARIES}
-# ${PROJECT_NAME})
-# ENDIF(Suitesparse_FOUND)
- ENDIF (laser_scan_utils_FOUND)
-ENDIF(faramotics_FOUND)
+# IF(faramotics_FOUND)
+# IF (laser_scan_utils_FOUND)
+# ADD_EXECUTABLE(test_ceres_2_lasers test_ceres_2_lasers.cpp)
+# TARGET_LINK_LIBRARIES(test_ceres_2_lasers
+# ${pose_state_time_LIBRARIES}
+# ${faramotics_LIBRARIES}
+# ${PROJECT_NAME})
+# ADD_EXECUTABLE(test_ceres_2_lasers_polylines test_ceres_2_lasers_polylines.cpp)
+# TARGET_LINK_LIBRARIES(test_ceres_2_lasers_polylines
+# ${pose_state_time_LIBRARIES}
+# ${faramotics_LIBRARIES}
+# ${PROJECT_NAME})
+# ADD_EXECUTABLE(test_2_lasers_offline test_2_lasers_offline.cpp)
+# TARGET_LINK_LIBRARIES(test_2_lasers_offline
+# ${pose_state_time_LIBRARIES}
+# ${faramotics_LIBRARIES}
+# ${PROJECT_NAME})
+# ADD_EXECUTABLE(test_faramotics_simulation test_faramotics_simulation.cpp)
+# TARGET_LINK_LIBRARIES(test_faramotics_simulation
+# ${pose_state_time_LIBRARIES}
+# ${faramotics_LIBRARIES}
+# ${PROJECT_NAME})
+# # ADD_EXECUTABLE(test_autodiff test_autodiff.cpp)
+# # TARGET_LINK_LIBRARIES(test_autodiff
+# # ${pose_state_time_LIBRARIES}
+# # ${faramotics_LIBRARIES}
+# # ${PROJECT_NAME})
+# # IF(Suitesparse_FOUND)
+# # ADD_EXECUTABLE(test_iQR_wolf2 solver/test_iQR_wolf2.cpp)
+# # TARGET_LINK_LIBRARIES(test_iQR_wolf2
+# # ${pose_state_time_LIBRARIES}
+# # ${faramotics_LIBRARIES}
+# # ${PROJECT_NAME})
+# # ENDIF(Suitesparse_FOUND)
+# # ENDIF (laser_scan_utils_FOUND)
+# # ENDIF(faramotics_FOUND)
diff --git a/src/feature_match.h b/src/feature_match.h
new file mode 100644
index 0000000000000000000000000000000000000000..b447adbb4d73eb3deec2feb676cbb47b287dde81
--- /dev/null
+++ b/src/feature_match.h
@@ -0,0 +1,28 @@
+#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
+
+
diff --git a/src/landmark_match.h b/src/landmark_match.h
new file mode 100644
index 0000000000000000000000000000000000000000..26b0b6f9206a1b479bd833a5a214b58bff4f0ab2
--- /dev/null
+++ b/src/landmark_match.h
@@ -0,0 +1,38 @@
+#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
diff --git a/src/processor_image_feature.cpp b/src/processor_image_feature.cpp
index b54e949526afe46b355e8da80e90542a1d98ce82..7bb3ce319a217dfe006b09160f0a4c547ff77635 100644
--- a/src/processor_image_feature.cpp
+++ b/src/processor_image_feature.cpp
@@ -144,7 +144,7 @@ unsigned int ProcessorImageFeature::trackFeatures(const FeatureBaseList& _featur
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
{
diff --git a/src/processor_tracker_feature.cpp b/src/processor_tracker_feature.cpp
index 953b6c1c6ce911152e5b55fd386d191d1995ad46..235d6688a91c8ce734b551dd2c2f47c5e078cf7a 100644
--- a/src/processor_tracker_feature.cpp
+++ b/src/processor_tracker_feature.cpp
@@ -39,8 +39,7 @@ unsigned int ProcessorTrackerFeature::processKnown()
auto known_incoming_feature_it = known_features_incoming_.begin();
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_,
- matches_last_from_incoming_[*known_incoming_feature_it].feature_ptr_,*known_incoming_feature_it))
+ 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))
{
// Correspondence not confirmed -> Remove correspondence and destruct incoming feature
matches_last_from_incoming_.erase(*known_incoming_feature_it);
diff --git a/src/processor_tracker_feature.h b/src/processor_tracker_feature.h
index 963742eaf492b9aeaa616aa7009849f7a3485605..84b252bde3ab147b66b2969a3d0e8c6fcddf926e 100644
--- a/src/processor_tracker_feature.h
+++ b/src/processor_tracker_feature.h
@@ -8,8 +8,10 @@
#ifndef PROCESSOR_TRACKER_FEATURE_H_
#define PROCESSOR_TRACKER_FEATURE_H_
+//wolf includes
#include "processor_tracker.h"
#include "capture_base.h"
+#include "feature_match.h"
#include "wolf.h"
namespace wolf
@@ -172,10 +174,10 @@ inline void ProcessorTrackerFeature::establishConstraints()
{
for (auto match : matches_origin_from_last_)
{
- auto ctr = createConstraint(match.first, match.second.feature_ptr_);
- ctr->setFeatureOtherPtr(match.second.feature_ptr_);
+ auto ctr = createConstraint(match.first, match.second->feature_ptr_);
+ ctr->setFeatureOtherPtr(match.second->feature_ptr_);
match.first->addConstraint(ctr);
- match.second.feature_ptr_->addConstrainedBy(ctr);
+ match.second->feature_ptr_->addConstrainedBy(ctr);
}
}
@@ -187,7 +189,7 @@ inline void ProcessorTrackerFeature::advance()
for (auto match : matches_last_from_incoming_)
{
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_);
diff --git a/src/processor_tracker_feature_corner.cpp b/src/processor_tracker_feature_corner.cpp
index 9c80d497e6a017775a9f74cb00eb76476bb90f95..c63ebe5efac9e29b7c3f54f6fdd929b981edbdf4 100644
--- a/src/processor_tracker_feature_corner.cpp
+++ b/src/processor_tracker_feature_corner.cpp
@@ -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_)
{
// 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
_feature_list_out.splice(_feature_list_out.end(), corners_incoming_, feat_out_it);
diff --git a/src/processor_tracker_feature_dummy.cpp b/src/processor_tracker_feature_dummy.cpp
index 71decb6aa29225cba6aeec4851d6990c03e5ba32..6a7bba038afe364879eb5ddfa14b7d149a7348e6 100644
--- a/src/processor_tracker_feature_dummy.cpp
+++ b/src/processor_tracker_feature_dummy.cpp
@@ -29,7 +29,7 @@ unsigned int ProcessorTrackerFeatureDummy::trackFeatures(const FeatureBaseList&
else
{
_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;
}
}
diff --git a/src/processor_tracker_landmark.h b/src/processor_tracker_landmark.h
index 96d7c2cbd9fefada97840dd8c1a010369e21260c..5e758c76d4dc1bb324ac600decf1aee696c54518 100644
--- a/src/processor_tracker_landmark.h
+++ b/src/processor_tracker_landmark.h
@@ -8,8 +8,10 @@
#ifndef PROCESSOR_TRACKER_LANDMARK_H_
#define PROCESSOR_TRACKER_LANDMARK_H_
+//wolf includes
#include "processor_tracker.h"
#include "capture_base.h"
+#include "landmark_match.h"
namespace wolf
{
diff --git a/src/wolf.h b/src/wolf.h
index 7d1bfd7edc1a13631c0ea9ef95b8e51e213a7c62..c9a8711f1b7b6de2116c40b55a4831c0eaba6462 100644
--- a/src/wolf.h
+++ b/src/wolf.h
@@ -378,36 +378,14 @@ typedef std::shared_ptr<StateQuaternion> StateQuaternionPtr;
// - - Local Parametrization
typedef std::shared_ptr<LocalParametrizationBase> LocalParametrizationBasePtr;
-// - - Match Feature - 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)
- {
-
- }
-};
-
-// 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;
+// // Feature-Feature correspondence
+// struct FeatureMatch
+// {
+// FeatureBasePtr feature_ptr_;
+// Scalar normalized_score_;
+// };
+//
+// typedef std::map<FeatureBasePtr, FeatureMatch> FeatureMatchMap;