From 0211bba1f4db2e19af70cd82207867c0ef88abe6 Mon Sep 17 00:00:00 2001
From: Joan Sola <jsola@iri.upc.edu>
Date: Thu, 21 Apr 2022 09:10:26 +0200
Subject: [PATCH] Use noise params from sensor not from processor
---
include/vision/processor/processor_visual_odometry.h | 3 ---
include/vision/sensor/sensor_camera.h | 10 ++++------
src/processor/processor_visual_odometry.cpp | 12 ++----------
src/sensor/sensor_camera.cpp | 5 +++--
4 files changed, 9 insertions(+), 21 deletions(-)
diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index 8544e39ed..162e15348 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 91b23e254..a70c43ce7 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 a0ea2f2b3..03276720a 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 8c57c542e..b346f7403 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_);
--
GitLab