diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h index 8544e39edd590cdfb191f35e942d393ec021199c..162e153480a341aa4e94cb24406a5e1d9036621e 100644 --- a/include/vision/processor/processor_visual_odometry.h +++ b/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"); diff --git a/include/vision/sensor/sensor_camera.h b/include/vision/sensor/sensor_camera.h index 91b23e254d9f86d7f4c621a5a4a49c26d1cf287b..a70c43ce7a885979e540ee50d4f53119a5f84889 100644 --- a/include/vision/sensor/sensor_camera.h +++ b/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) { diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp index a0ea2f2b38a0df188f2d24201b14319d571d9072..03276720a726a8a9c0e9d09d7152c419a52c2825 100644 --- a/src/processor/processor_visual_odometry.cpp +++ b/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(); diff --git a/src/sensor/sensor_camera.cpp b/src/sensor/sensor_camera.cpp index 8c57c542eae854c48476ad4caf1023fc7bac1ec0..b346f7403ac7a267d3bec848344ace4dca08b8a1 100644 --- a/src/sensor/sensor_camera.cpp +++ b/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_);