From 7ef323a45a7a9d8b47216e78c1266b8dc95b9206 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Joan=20Sol=C3=A0?= <jsola@iri.upc.edu>
Date: Fri, 12 Aug 2022 12:14:25 +0200
Subject: [PATCH] Vote by fitness score
---
include/laser/processor/processor_odom_icp_3d.h | 2 ++
src/processor/processor_odom_icp_3d.cpp | 16 +++++++++++++---
test/yaml/problem_odom_icp_3d.yaml | 3 ++-
3 files changed, 17 insertions(+), 4 deletions(-)
diff --git a/include/laser/processor/processor_odom_icp_3d.h b/include/laser/processor/processor_odom_icp_3d.h
index ecb8d3690..cbeaf7967 100644
--- a/include/laser/processor/processor_odom_icp_3d.h
+++ b/include/laser/processor/processor_odom_icp_3d.h
@@ -51,6 +51,7 @@ WOLF_STRUCT_PTR_TYPEDEFS(ParamsProcessorOdomIcp3d);
struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMotionProvider
{
double max_time_span;
+ double max_fitness_score; // maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
bool pcl_downsample;
double pcl_downsample_voxel_size;
@@ -68,6 +69,7 @@ struct ParamsProcessorOdomIcp3d : public ParamsProcessorTracker, public ParamsMo
: ParamsProcessorTracker(_unique_name, _server), ParamsMotionProvider(_unique_name, _server)
{
max_time_span = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_time_span");
+ max_fitness_score = _server.getParam<double>(prefix + _unique_name + "/keyframe_vote/max_fitness_score");
pcl_downsample = _server.getParam<bool>(prefix + _unique_name + "/pcl_downsample");
pcl_downsample_voxel_size = _server.getParam<double>(prefix + _unique_name + "/pcl_downsample_voxel_size");
diff --git a/src/processor/processor_odom_icp_3d.cpp b/src/processor/processor_odom_icp_3d.cpp
index 95f249f5c..8b10b650c 100644
--- a/src/processor/processor_odom_icp_3d.cpp
+++ b/src/processor/processor_odom_icp_3d.cpp
@@ -179,14 +179,24 @@ unsigned int ProcessorOdomIcp3d::processNew(const int& _max_features)
*/
bool ProcessorOdomIcp3d::voteForKeyFrame() const
{
+ // 1. vote by time
if (last_ptr_->getTimeStamp() - origin_ptr_->getTimeStamp() > params_odom_icp_->max_time_span)
{
+ WOLF_DEBUG("ProcessorOdomIcp3d: vote KF by time");
return true;
}
// TODO:
- // - vote by distance
- // - vote by angle
- // - vote by nbr. of captures
+ // 2. vote by distance
+ // 3. vote by angle
+ // 4. vote by nbr. of captures
+
+ // 5. vote by fitness score
+ if (registration_solver_->getFitnessScore() > params_odom_icp_->max_fitness_score)
+ {
+ WOLF_DEBUG("ProcessorOdomIcp3d: vote KF by fitness score");
+ return true;
+ }
+
return false;
};
diff --git a/test/yaml/problem_odom_icp_3d.yaml b/test/yaml/problem_odom_icp_3d.yaml
index 02f558aaa..8291a5714 100644
--- a/test/yaml/problem_odom_icp_3d.yaml
+++ b/test/yaml/problem_odom_icp_3d.yaml
@@ -37,6 +37,7 @@ config:
voting_active: true
min_features_for_keyframe : 0
max_time_span: 0.99
+ max_fitness_score: 0.0001 # maximum Euclidean fitness score (e.g., mean of squared distances from the source to the target)
angle_turned: 1.0
state_getter: true
state_priority: 1
@@ -44,7 +45,7 @@ config:
pcl_downsample_voxel_size: 0.05
icp_algorithm: "icp_nl" # "icp", "icp_nl", "gicp"
icp_max_iterations: 20
- icp_transformation_rotation_epsilon: 0.99 # this is cos(alpha)
+ icp_transformation_rotation_epsilon: 0.9999 # this is cos(alpha)
icp_transformation_translation_epsilon: 1e-9 # this is translation squared
icp_max_correspondence_distance: 0.05
--
GitLab