Add noise parameters in yaml

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

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]);

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]);

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

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>();