From a1bea436987cb13e1a69911837418f0c10cde628 Mon Sep 17 00:00:00 2001
From: Mederic Fourmy <mederic.fourmy@gmail.com>
Date: Tue, 19 Apr 2022 17:55:57 +0200
Subject: [PATCH] Added ransac params to the processor
---
demos/processor_visual_odometry.yaml | 17 ++++++++
.../processor/processor_visual_odometry.h | 12 ++++++
src/processor/processor_visual_odometry.cpp | 13 +++---
test/gtest_processor_visual_odometry.cpp | 11 +++--
test/processor_visual_odometry_gtest.yaml | 42 ++++++++++++-------
5 files changed, 69 insertions(+), 26 deletions(-)
diff --git a/demos/processor_visual_odometry.yaml b/demos/processor_visual_odometry.yaml
index d0be856d2..53b0c4123 100644
--- a/demos/processor_visual_odometry.yaml
+++ b/demos/processor_visual_odometry.yaml
@@ -36,6 +36,23 @@ klt_params:
nlevels_pyramids: 3
klt_max_err: 0.3
+# tesselation grid
+grid_params:
+ # number of cells used by the active search grid data structure
+ nbr_cells_h: 6 # horizontal
+ nbr_cells_v: 5 # vertical
+ # minimum margin of the region of interest from the edges of the image
+ margin: 10
+ # reduce the size of each region of interest by n pixels to prevent keypoints from being too close
+ separation: 10
+
+ransac_params:
+ # specifies a desirable level of confidence (probability) that the estimated matrix is correct
+ ransac_prob: 0.99
+ # maximum distance from a point to an epipolar line in pixels, beyond which the point
+ # is considered an outlier and is not used for computing the final fundamental matrix
+ ransac_thresh: 1
+
# Keep the number of tracks below
max_nb_tracks: 100
diff --git a/include/vision/processor/processor_visual_odometry.h b/include/vision/processor/processor_visual_odometry.h
index 4ff307ddf..8544e39ed 100644
--- a/include/vision/processor/processor_visual_odometry.h
+++ b/include/vision/processor/processor_visual_odometry.h
@@ -52,6 +52,12 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorVisualOdometry);
struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
{
+ struct RansacParams
+ {
+ double ransac_prob_;
+ double ransac_thresh_;
+ };
+
struct KltParams
{
int patch_width_;
@@ -76,6 +82,7 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
};
double std_pix_;
+ RansacParams ransac_params_;
KltParams klt_params_;
FastParams fast_params_;
GridParams grid_params_;
@@ -88,6 +95,9 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
{
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");
+
klt_params_.patch_width_ = _server.getParam<int>(prefix + _unique_name + "/klt_params/patch_width");
klt_params_.patch_height_ = _server.getParam<int>(prefix + _unique_name + "/klt_params/patch_height");
klt_params_.klt_max_err_ = _server.getParam<double>(prefix + _unique_name + "/klt_params/klt_max_err");
@@ -109,6 +119,8 @@ struct ParamsProcessorVisualOdometry : public ParamsProcessorTracker
std::string print() const override
{
return ParamsProcessorTracker::print() + "\n"
+ + "ransac_params_.ransac_prob_: " + std::to_string(ransac_params_.ransac_prob_) + "\n"
+ + "ransac_params_.ransac_thresh_: " + std::to_string(ransac_params_.ransac_thresh_) + "\n"
+ "klt_params_.tracker_width_: " + std::to_string(klt_params_.patch_width_) + "\n"
+ "klt_params_.tracker_height_: " + std::to_string(klt_params_.patch_height_) + "\n"
+ "klt_params_.klt_max_err_: " + std::to_string(klt_params_.klt_max_err_) + "\n"
diff --git a/src/processor/processor_visual_odometry.cpp b/src/processor/processor_visual_odometry.cpp
index 7e3179a1b..a0ea2f2b3 100644
--- a/src/processor/processor_visual_odometry.cpp
+++ b/src/processor/processor_visual_odometry.cpp
@@ -644,6 +644,7 @@ TracksMap ProcessorVisualOdometry::kltTrack(const cv::Mat _img_prev, const cv::M
bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev, const KeyPointsMap _mwkps_curr, TracksMap &_tracks_prev_curr, cv::Mat &_E)
{
+ ParamsProcessorVisualOdometry::RansacParams prms = params_visual_odometry_->ransac_params_;
// We need to build lists of pt2f for openCV function
std::vector<cv::Point2f> p2f_prev, p2f_curr;
@@ -659,12 +660,12 @@ bool ProcessorVisualOdometry::filterWithEssential(const KeyPointsMap _mwkps_prev
cv::Mat cvMask;
_E = cv::findEssentialMat(p2f_prev,
- p2f_curr,
- Kcv_,
- cv::RANSAC,
- 0.999,
- 1.0,
- cvMask);
+ p2f_curr,
+ Kcv_,
+ cv::RANSAC,
+ prms.ransac_prob_,
+ prms.ransac_thresh_,
+ cvMask);
// Let's remove outliers from tracksMap
for (size_t k=0; k<all_indices.size(); k++){
diff --git a/test/gtest_processor_visual_odometry.cpp b/test/gtest_processor_visual_odometry.cpp
index 672a519b4..4a584eb7a 100644
--- a/test/gtest_processor_visual_odometry.cpp
+++ b/test/gtest_processor_visual_odometry.cpp
@@ -32,6 +32,7 @@
#include <core/yaml/parser_yaml.h>
#include <opencv2/imgcodecs.hpp>
+#include "core/math/rotations.h"
#include "vision/processor/processor_visual_odometry.h"
#include "vision/sensor/sensor_camera.h"
@@ -343,6 +344,8 @@ TEST(ProcessorVisualOdometry, filterWithEssential)
params->grid_params_.nbr_cells_h_ = 8;
params->grid_params_.nbr_cells_v_ = 8;
params->grid_params_.separation_ = 10;
+ params->ransac_params_.ransac_prob_ = 0.999; // 0.99 makes the gtest fail -> missing one point
+ params->ransac_params_.ransac_thresh_ = 1.0;
ProcessorVisualOdometryTest processor(params);
// Install camera
@@ -375,10 +378,10 @@ TEST(ProcessorVisualOdometry, filterWithEssential)
// Set the transformation between the two views
Eigen::Vector3f t(0.1, 0.1, 0.0);
- Eigen::Matrix3f R;
- R << 0.7, -0.7, 0.0,
- 0.7, 0.7, 0.0,
- 0.0, 0.0, 1.0;
+ // Eigen::Vector3f euler(0.0, 0.0, 0.0); // degenerate case!
+ Eigen::Vector3f euler(0.2, 0.1, 0.3);
+ Eigen::Matrix3f R = e2R(euler);
+
// Project pixels in the current view
mwkps_curr.insert(std::pair<size_t, WKeyPoint>(0, WKeyPoint(cv::KeyPoint(project(K,R*X1_prev + t), 1))));
diff --git a/test/processor_visual_odometry_gtest.yaml b/test/processor_visual_odometry_gtest.yaml
index 427fa2fd8..c242cd38b 100644
--- a/test/processor_visual_odometry_gtest.yaml
+++ b/test/processor_visual_odometry_gtest.yaml
@@ -4,13 +4,13 @@ keyframe_vote:
voting_active: true
# Trigger a new keyframe creation as well as detection of new keypoints in last frame
# when the track number goes below min_features_for_keyframe in incoming
- min_features_for_keyframe: 10
+ min_features_for_keyframe: 25
#Â Use a robust cost function
apply_loss_function: true
# Select the best new Keypoints when performing detection
-max_new_features: 100
+max_new_features: 4
####################################
# ProcessorVisualOdometry parameters
@@ -18,29 +18,39 @@ max_new_features: 100
fast_params:
# Threshold on the keypoint pixel intensity (in uchar [0-255])
# the higher, the more selective the detector is
- threshold_fast: 50
+ threshold_fast: 10
# Avoids getting multiple keypoints at the same place
- non_max_suppresion: true
- # number of cells used by the active search grid data structure
- active_search_grid_nb_h: 8 # horizontal
- active_search_grid_nb_v: 8 # vertical
- # minimum margin of the region of interest from the edges of the image
- active_search_margin: 10
- # reduce the size of each region of interest by n pixels to prevent keypoints from being too close
- active_search_separation: 10
+ non_max_suppresion: false
# Lucas Kanade tracking parameters
klt_params:
- patch_width: 15
- patch_height: 15
+ patch_width: 21
+ patch_height: 21
nlevels_pyramids: 3
- klt_max_err: 0.3
+ klt_max_err: 0.2
+
+# tesselation grid
+grid_params:
+ # number of cells used by the active search grid data structure
+ nbr_cells_h: 6 # horizontal
+ nbr_cells_v: 5 # vertical
+ # minimum margin of the region of interest from the edges of the image
+ margin: 10
+ # reduce the size of each region of interest by n pixels to prevent keypoints from being too close
+ separation: 10
+
+ransac_params:
+ # specifies a desirable level of confidence (probability) that the estimated matrix is correct
+ ransac_prob: 0.99
+ # maximum distance from a point to an epipolar line in pixels, beyond which the point
+ # is considered an outlier and is not used for computing the final fundamental matrix
+ ransac_thresh: 1
# Keep the number of tracks below
-max_nb_tracks: 20
+max_nb_tracks: 30
# standard deviation of the pixel reprojection factor
std_pix: 1
# before creating a landmark, wait until the track is old enough
-min_track_length_for_landmark: 3
+min_track_length_for_landmark: 20
--
GitLab