diff --git a/src/examples/processor_image_ORB.yaml b/src/examples/processor_image_ORB.yaml index d7a4a6c2e87d4094f56eb9f93650f451800c0a83..3839dc8b3b47cdd1f6c7fa99a68ee28367b50b89 100644 --- a/src/examples/processor_image_ORB.yaml +++ b/src/examples/processor_image_ORB.yaml @@ -31,6 +31,9 @@ algorithm: time tolerance: 0.01 distance: 20 +noise: + pixel noise std: 1 # pixels + draw: # Used to control drawing options primary draw: true secondary draw: true diff --git a/src/processor_image_feature.cpp b/src/processor_image_feature.cpp index 8b4de59670ef39ab6cf7967faa52cdec3e9cf237..974b81fea524994252e1c8a5d2f12fef919994d7 100644 --- a/src/processor_image_feature.cpp +++ b/src/processor_image_feature.cpp @@ -139,7 +139,8 @@ unsigned int ProcessorImageFeature::trackFeatures(const FeatureBaseList& _featur { FeaturePointImage::Ptr incoming_point_ptr = std::make_shared<FeaturePointImage>( candidate_keypoints[cv_matches[0].trainIdx], (candidate_descriptors.row(cv_matches[0].trainIdx)), - feature_ptr->isKnown()); + Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var); + incoming_point_ptr->setIsKnown(feature_ptr->isKnown()); _feature_list_out.push_back(incoming_point_ptr); incoming_point_ptr->setTrackId(feature_ptr->trackId()); @@ -244,7 +245,9 @@ unsigned int ProcessorImageFeature::detectNewFeatures(const unsigned int& _max_n if(new_keypoints[0].response > params_.algorithm.min_response_for_new_features) { std::cout << "response: " << new_keypoints[0].response << std::endl; - FeaturePointImage::Ptr point_ptr = std::make_shared<FeaturePointImage>(new_keypoints[0], new_descriptors.row(index), false); + FeaturePointImage::Ptr point_ptr = std::make_shared<FeaturePointImage>(new_keypoints[0], new_descriptors.row(index), + Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var); + point_ptr->setIsKnown(false); point_ptr->setTrackId(point_ptr->id()); addNewFeatureLast(point_ptr); active_search_grid_.hitCell(new_keypoints[0]); diff --git a/src/processor_image_landmark.cpp b/src/processor_image_landmark.cpp index 67d39dcaced64f19b8526894907c020b7c04bed1..f5afce9d2caf2356f6e54f12e2cadd5d5dcc8127 100644 --- a/src/processor_image_landmark.cpp +++ b/src/processor_image_landmark.cpp @@ -166,7 +166,7 @@ unsigned int ProcessorImageLandmark::findLandmarks(const LandmarkBaseList& _land std::shared_ptr<FeaturePointImage> incoming_point_ptr = std::make_shared<FeaturePointImage>(candidate_keypoints[cv_matches[0].trainIdx], - (candidate_descriptors.row(cv_matches[0].trainIdx)), Eigen::Matrix2s::Identity()); + (candidate_descriptors.row(cv_matches[0].trainIdx)), Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var); incoming_point_ptr->setTrackId(landmark_in_ptr->id()); incoming_point_ptr->setLandmarkId(landmark_in_ptr->id()); _feature_list_out.push_back(incoming_point_ptr); @@ -229,7 +229,9 @@ unsigned int ProcessorImageLandmark::detectNewFeatures(const unsigned int& _max_ if(new_keypoints[0].response > params_.algorithm.min_response_for_new_features) { list_response_.push_back(new_keypoints[0].response); - std::shared_ptr<FeaturePointImage> point_ptr = std::make_shared<FeaturePointImage>(new_keypoints[0], new_descriptors.row(index), false); + std::shared_ptr<FeaturePointImage> point_ptr = std::make_shared<FeaturePointImage>(new_keypoints[0], new_descriptors.row(index), + Eigen::Matrix2s::Identity()*params_.noise.pixel_noise_var); + point_ptr->setIsKnown(false); point_ptr->setTrackId(point_ptr->id()); addNewFeatureLast(point_ptr); active_search_grid_.hitCell(new_keypoints[0]); diff --git a/src/processor_image_params.h b/src/processor_image_params.h index 07d5b60d79ae52b3f217bea98cd757d063103c89..b17351cc7be8843c4ff8698ec0aec78d5192c452 100644 --- a/src/processor_image_params.h +++ b/src/processor_image_params.h @@ -67,6 +67,12 @@ struct ProcessorParamsImage : public ProcessorParamsBase Scalar distance; }algorithm; + struct Noise + { + Scalar pixel_noise_std; ///< std noise of the pixel + Scalar pixel_noise_var; ///< var noise of the pixel + }noise; + struct Draw { bool primary_drawing; ///< draw the features found in the image diff --git a/src/yaml/processor_image_yaml.cpp b/src/yaml/processor_image_yaml.cpp index 82b031dfc6c543d03832d5d0126257b5d477b7b5..9a5e344879f023b88d77cbefe532be842bc207df 100644 --- a/src/yaml/processor_image_yaml.cpp +++ b/src/yaml/processor_image_yaml.cpp @@ -68,6 +68,10 @@ static ProcessorParamsBasePtr createProcessorParamsImage(const std::string & _fi p->algorithm.time_tolerance= alg["time tolerance"].as<Scalar>(); p->algorithm.distance= alg["distance"].as<Scalar>(); + Node noi = params["noise"]; + p->noise.pixel_noise_std = noi["pixel noise std"].as<Scalar>(); + p->noise.pixel_noise_var = p->noise.pixel_noise_std * p->noise.pixel_noise_std; + Node draw = params["draw"]; p->draw.primary_drawing = draw["primary draw"].as<bool>(); p->draw.secondary_drawing = draw["secondary draw"].as<bool>();