Use noise params from sensor not from processor

include/vision/processor/processor_visual_odometry.h

@@ -81,7 +81,6 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
         unsigned int separation_;
     };
 
-    double std_pix_;
     RansacParams ransac_params_;
     KltParams klt_params_;
     FastParams fast_params_;
@@ -93,8 +92,6 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
     ParamsProcessorVisualOdometry(std::string _unique_name, const ParamsServer& _server):
         ParamsProcessorTracker(_unique_name, _server)
     {
-        std_pix_ = _server.getParam<int>(prefix + _unique_name + "/std_pix");
-
         ransac_params_.ransac_prob_   = _server.getParam<double>(prefix + _unique_name + "/ransac_params/ransac_prob");
         ransac_params_.ransac_thresh_ = _server.getParam<double>(prefix + _unique_name + "/ransac_params/ransac_thresh");
 

include/vision/sensor/sensor_camera.h

@@ -23,8 +23,8 @@
 #define SENSOR_CAMERA_H
 
 //wolf includes
-#include "core/sensor/sensor_base.h"
-#include "core/utils/params_server.h"
+#include <core/sensor/sensor_base.h>
+#include <core/utils/params_server.h>
 
 namespace wolf
 {
@@ -39,10 +39,8 @@ struct ParamsSensorCamera : public ParamsSensorBase
         Eigen::Vector4d pinhole_model_raw;      ///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
         Eigen::Vector4d pinhole_model_rectified;///< k = [u_0, v_0, alpha_u, alpha_v]  vector of pinhole intrinsic parameters
         Eigen::VectorXd distortion;             ///< d = [d_1, d_2, d_3, ...] radial distortion coefficients
-    ParamsSensorCamera()
-    {
-        //DEFINED FOR COMPATIBILITY PURPOSES. TO BE REMOVED IN THE FUTURE.
-    }
+    ParamsSensorCamera() = default;
+
     ParamsSensorCamera(std::string _unique_name, const ParamsServer& _server):
         ParamsSensorBase(_unique_name,  _server)
     {

src/processor/processor_visual_odometry.cpp

@@ -38,12 +38,6 @@ ProcessorVisualOdometry::ProcessorVisualOdometry(ParamsProcessorVisualOdometryPt
     detector_ = cv::FastFeatureDetector::create(_params_vo->fast_params_.threshold_fast_, 
                                                 _params_vo->fast_params_.non_max_suppresion_,
                                                 cv::FastFeatureDetector::TYPE_9_16); // TYPE_5_8, TYPE_7_12, TYPE_9_16
-    
-    // Processor stuff
-    // Set pixel noise covariance
-    Eigen::Vector2d std_pix; std_pix << params_visual_odometry_->std_pix_, params_visual_odometry_->std_pix_;
-    pixel_cov_ = std_pix.array().square().matrix().asDiagonal();
-
 }
 
 void ProcessorVisualOdometry::configure(SensorBasePtr _sensor)
@@ -63,6 +57,8 @@ void ProcessorVisualOdometry::configure(SensorBasePtr _sensor)
                                   params_visual_odometry_->grid_params_.nbr_cells_v_,
                                   params_visual_odometry_->grid_params_.margin_,
                                   params_visual_odometry_->grid_params_.separation_);
+
+    pixel_cov_ = sen_cam_->getNoiseCov();
 }
 
 TracksMap ProcessorVisualOdometry::mergeTracks(const TracksMap& tracks_prev_curr, const TracksMap& tracks_curr_next){
@@ -302,10 +298,6 @@ void ProcessorVisualOdometry::preProcess()
         // add a flag so that voteForKeyFrame use it to vote for a KeyFrame 
         capture_image_incoming_->setLastWasRepopulated(true);
     }
-    else
-    {
-        WOLF_INFO("\n\n");
-    }
 
     auto dt_preprocess = std::chrono::duration_cast<std::chrono::milliseconds>(std::chrono::system_clock::now() - t1).count();
 

src/sensor/sensor_camera.cpp

@@ -23,8 +23,8 @@
 #include "vision/sensor/sensor_camera.h"
 #include "vision/math/pinhole_tools.h"
 
-#include "core/state_block/state_block.h"
-#include "core/state_block/state_quaternion.h"
+#include <core/state_block/state_block.h>
+#include <core/state_block/state_quaternion.h>
 
 namespace wolf
 {
@@ -43,6 +43,7 @@ SensorCamera::SensorCamera(const Eigen::VectorXd& _extrinsics, const ParamsSenso
                 pinhole_model_rectified_(_intrinsics.pinhole_model_rectified), //
                 using_raw_(true)
 {
+    setNoiseStd(_intrinsics.noise_std);
     assert(_extrinsics.size() == 7 && "Wrong intrinsics vector size. Should be 7 for 3d");
     useRawImages();
     pinhole::computeCorrectionModel(getIntrinsic()->getState(), distortion_, correction_);