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

Merge branch 'tracker_landmark_corner' into 'master'

fix corner 2D

See merge request !115
parents 90ea23e1 e42cac69
No related branches found
No related tags found
1 merge request!115fix corner 2D
...@@ -4,7 +4,7 @@ ...@@ -4,7 +4,7 @@
namespace wolf { namespace wolf {
FeatureCorner2D::FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance) : FeatureCorner2D::FeatureCorner2D(const Eigen::Vector4s & _measurement, const Eigen::Matrix4s & _meas_covariance) :
FeatureBase("CORNER", _measurement, _meas_covariance) FeatureBase("CORNER 2D", _measurement, _meas_covariance)
{ {
//std::cout << "feature: "<< _measurement.transpose() << std::endl; //std::cout << "feature: "<< _measurement.transpose() << std::endl;
} }
......
...@@ -96,7 +96,7 @@ ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr ...@@ -96,7 +96,7 @@ ConstraintBasePtr ProcessorTrackerFeatureCorner::createConstraint(FeatureBasePtr
// Getting landmark ptr // Getting landmark ptr
LandmarkCorner2DPtr landmark_ptr = nullptr; LandmarkCorner2DPtr landmark_ptr = nullptr;
for (auto constraint : _feature_other_ptr->getConstraintList()) for (auto constraint : _feature_other_ptr->getConstraintList())
if (constraint->getLandmarkOtherPtr() != nullptr && constraint->getLandmarkOtherPtr()->getType() == "CORNER") if (constraint->getLandmarkOtherPtr() != nullptr && constraint->getLandmarkOtherPtr()->getType() == "CORNER 2D")
landmark_ptr = std::static_pointer_cast<LandmarkCorner2D>(constraint->getLandmarkOtherPtr()); landmark_ptr = std::static_pointer_cast<LandmarkCorner2D>(constraint->getLandmarkOtherPtr());
if (landmark_ptr == nullptr) if (landmark_ptr == nullptr)
......
...@@ -64,7 +64,7 @@ unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseLis ...@@ -64,7 +64,7 @@ unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseLis
Scalar closest_dm2 = 1e3; Scalar closest_dm2 = 1e3;
for (auto landmark_it = not_matched_landmarks.begin(); landmark_it != not_matched_landmarks.end(); landmark_it++) for (auto landmark_it = not_matched_landmarks.begin(); landmark_it != not_matched_landmarks.end(); landmark_it++)
{ {
if ((*landmark_it)->getType() == "CORNER" && if ((*landmark_it)->getType() == "CORNER 2D" &&
fabs(pi2pi((std::static_pointer_cast<FeatureCorner2D>(*feature_it))->getAperture() - (*landmark_it)->getDescriptor(0))) < aperture_error_th_) fabs(pi2pi((std::static_pointer_cast<FeatureCorner2D>(*feature_it))->getAperture() - (*landmark_it)->getDescriptor(0))) < aperture_error_th_)
{ {
dm2 = computeSquaredMahalanobisDistances((*feature_it), expected_features[*landmark_it], dm2 = computeSquaredMahalanobisDistances((*feature_it), expected_features[*landmark_it],
...@@ -128,7 +128,7 @@ unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseLis ...@@ -128,7 +128,7 @@ unsigned int ProcessorTrackerLandmarkCorner::findLandmarks(const LandmarkBaseLis
for (auto landmark_it = _landmarks_corner_searched.begin(); landmark_it != _landmarks_corner_searched.end(); for (auto landmark_it = _landmarks_corner_searched.begin(); landmark_it != _landmarks_corner_searched.end();
landmark_it++, jj++) landmark_it++, jj++)
{ {
if ((*landmark_it)->getType() == "CORNER") if ((*landmark_it)->getType() == "CORNER 2D")
{ {
landmarks_map[jj] = landmark_it; landmarks_map[jj] = landmark_it;
landmarks_index_map[jj] = 0; landmarks_index_map[jj] = 0;
......
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