From 386c6f8f637f02998f49e78a3a852fbd392a8d0c Mon Sep 17 00:00:00 2001
From: Sergi Pujol <spujol@iri.upc.edu>
Date: Mon, 31 May 2021 16:55:04 +0200
Subject: [PATCH] wip
---
deps/falkolib | 2 +-
src/loop_closure_falko.h | 19 ++-
test/gtest_loop_closure_falko.cpp | 243 +++++++++++++++++-------------
3 files changed, 148 insertions(+), 116 deletions(-)
diff --git a/deps/falkolib b/deps/falkolib
index 0017c6c..8f341e9 160000
--- a/deps/falkolib
+++ b/deps/falkolib
@@ -1 +1 @@
-Subproject commit 0017c6c5be963b923684bfd8eb6bd8b7641ba1b6
+Subproject commit 8f341e9f1739c445cd8352586d514e2e49e25718
diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h
index 453cdbe..c4f94ca 100644
--- a/src/loop_closure_falko.h
+++ b/src/loop_closure_falko.h
@@ -190,15 +190,20 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
{
mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0];
mid_point[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1];
- // new_scene->angle_rotation_.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index);
angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index);
- // std::cout << "angle_min :" << angle_min << std::endl;
- // std::cout << "angle_min :" << angle_step << std::endl;
- // std::cout << "new_scene->angle_rotation_[i] :" << angle_rotation[i] << std::endl;
- descriptors_list_rotated[i].rotate(angle_rotation[i]);
+ // descriptors_list_rotated[i].rotate(angle_rotation[i]);
+ // double orientation = new_scene->keypoints_list_[i].orientation + angle_rotation[i];
+ double orientation = angle_rotation[i];
+ // if (orientation < 0)
+ // {
+ // orientation = 2 * M_PI + orientation;
+ // }
+ descriptors_list_rotated[i].rotate(orientation);
+ // std::cout << "orientation : " << angle_rotation[i] << std::endl;
+ // std::cout << "orientation desc : " << orientation << std::endl;
new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]);
}
-
+
new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size();
// Compute Scene Area and Perimeter
int n = 3;
@@ -371,7 +376,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
// new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(),
// scene_2_falko->keypoints_list_.size());
- new_match->score = (double)matching_number / 10.0;
+ new_match->score = (double)matching_number;
new_match->transform_vector.head(2) = new_match->transform.translation();
new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle();
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index c711ab9..878ac4b 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -32,7 +32,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
ASSERT_EQ(firstPoint, 250);
- // Test extractScene
+ // Test extractScene2
auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan, laser_params));
auto new_scene2 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan2, laser_params));
int detectedKeypoints = new_scene->keypoints_list_.size();
@@ -94,59 +94,80 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber;
int circularSectorNumber = new_scene->descriptors_list_[0].circularSectorNumber;
- int acum_distance = 0;
+ int acum_distance = 0;
+ double asso_dist[key_1.size()][key_2.size()];
+ double min_dist_vector[key_1.size()];
for (int i = 0; i < desc_1.size(); i++)
{
+ double min_dist = 1000;
+ int asso_number;
for (int j = 0; j < desc_2.size(); j++)
{
- if (key_1[i].index == key_2[j].index + rot or key_1[i].index == key_2[j].index + rot + 1 or
- key_1[i].index == key_2[j].index - 1440 + rot)
- {
- std::cout << "pair : " << i << " , " << j
- << " , distance : " << desc_1[i].distance(desc_2[j]) << std::endl;
-
- acum_distance += desc_1[i].distance(desc_2[j]);
-
- // grid to x and y
- std::vector<int> x_pos, x_pos_rotated;
- std::vector<int> y_pos, y_pos_rotated;
-
- int desc_1_number = i;
- int desc_2_number = j;
- auto grid_1 = new_scene->descriptors_list_rotated[i].grid;
- auto grid_2 = new_scene_2->descriptors_list_rotated[j].grid;
-
- for (int i = 0; i < radialRingNumber; i++)
- for (int j = 0; j < circularSectorNumber; j++)
- {
- if (grid_1[i][j] > 0)
- {
- x_pos.push_back(i);
- y_pos.push_back(j);
- }
-
- if (grid_2[i][j] > 0)
- {
- x_pos_rotated.push_back(i);
- y_pos_rotated.push_back(j);
- }
- }
-
- // Plotting descriptors
- plt::title("NNMatcher BSC only keypoints");
- plt::subplot(2, 1, 1);
- plt::xlabel(" descriprtor not rotated");
- plt::plot(x_pos, y_pos, "ob");
-
- plt::subplot(2, 1, 2);
- plt::xlabel(" descriprtor rotated");
- plt::plot(x_pos_rotated, y_pos_rotated, "or");
- // plt::show();
+ acum_distance += desc_1[i].distance(desc_2[j]);
+ asso_dist[i][j] = desc_1[i].distance(desc_2[j]);
+ if (asso_dist[i][j] < min_dist)
+ {
+ min_dist = asso_dist[i][j];
+ asso_number = j;
}
+ std::cout << "pair : " << i << " , " << j << " , distance : " << asso_dist[i][j] << std::endl;
}
+ // std::cout << "pair : " << i << " , " << asso_number << " , distance : " << min_dist << std::endl;
}
- // std::cout << "acum_distance : " << acum_distance << std::endl;
+
+ // for (int i = 0; i < desc_1.size(); i++)
+ // {
+ // for (int j = 0; j < desc_2.size(); j++)
+ // {
+ // if (key_1[i].index == key_2[j].index + rot or key_1[i].index == key_2[j].index + rot + 1 or
+ // key_1[i].index == key_2[j].index - 1440 + rot)
+ // {
+
+ // std::cout << "pair : " << i << " , " << j
+ // << " , distance : " << desc_1[i].distance(desc_2[j]) << std::endl;
+
+ // acum_distance += desc_1[i].distance(desc_2[j]);
+
+ // // grid to x and y
+ // std::vector<int> x_pos, x_pos_rotated;
+ // std::vector<int> y_pos, y_pos_rotated;
+
+ // int desc_1_number = i;
+ // int desc_2_number = j;
+ // auto grid_1 = new_scene->descriptors_list_rotated[i].grid;
+ // auto grid_2 = new_scene_2->descriptors_list_rotated[j].grid;
+
+ // for (int i = 0; i < radialRingNumber; i++)
+ // for (int j = 0; j < circularSectorNumber; j++)
+ // {
+ // if (grid_1[i][j] > 0)
+ // {
+ // x_pos.push_back(i);
+ // y_pos.push_back(j);
+ // }
+
+ // if (grid_2[i][j] > 0)
+ // {
+ // x_pos_rotated.push_back(i);
+ // y_pos_rotated.push_back(j);
+ // }
+ // }
+
+ // // Plotting descriptors
+ // // plt::title("NNMatcher BSC only keypoints");
+ // // plt::subplot(2, 1, 1);
+ // // plt::xlabel(" descriprtor not rotated");
+ // // plt::plot(x_pos, y_pos, "ob");
+
+ // // plt::subplot(2, 1, 2);
+ // // plt::xlabel(" descriprtor rotated");
+ // // plt::plot(x_pos_rotated, y_pos_rotated, "or");
+ // // plt::show();
+ // }
+ // }
+ // }
+ std::cout << "acum_distance : " << acum_distance << std::endl;
ASSERT_EQ(acum_distance, 39);
}
@@ -154,7 +175,7 @@ TEST(loop_closure_falko, TestMatch)
{
// Initialization
int scan_size = 1440;
- LaserScan scan_1, scan_2, scan_3;
+ LaserScan scan_1, scan_2, scan_3, scan_target, scan_ref;
LaserScanParams laser_params;
laser_params.angle_min_ = 0;
@@ -191,76 +212,82 @@ TEST(loop_closure_falko, TestMatch)
ASSERT_EQ(asso_1_2.size(), 13);
- std::cout << " n associations : " << asso_1_2.size() << std::endl;
-
- for (int i = 0; i < match_1_2->associations.size(); i++)
+ // ** TEST WITH TARGET AND REFERENCE SCENE
+ std::cout << "scan size : " << target_scan_1.size() << std::endl;
+ for (int i = 0; i < target_scan_1.size(); i++)
{
- std::cout << "id first : " << match_1_2->associations[i].first << std::endl;
- std::cout << "id second : " << match_1_2->associations[i].second << std::endl;
+ scan_target.ranges_raw_.push_back(target_scan_1[i]);
+ scan_ref.ranges_raw_.push_back(reference_scan_1[i]);
}
- // auto key_1 = new_scene->keypoints_list_;
- // auto key_2 = new_scene_2->keypoints_list_;
- // auto desc_1 = new_scene->descriptors_list_rotated;
- // auto desc_2 = new_scene_2->descriptors_list_rotated;
-
- // int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber;
- // int circularSectorNumber = new_scene->descriptors_list_[0].circularSectorNumber;
-
- // int acum_distance = 0;
- // for (int i = 0; i < desc_1.size(); i++)
- // {
- // for (int j = 0; j < desc_2.size(); j++)
- // {
- // if (key_1[i].index == key_2[j].index + rot or key_1[i].index == key_2[j].index + rot + 1 or
- // key_1[i].index == key_2[j].index - 1440 + rot)
- // {
-
- // std::cout << "pair : " << i << " , " << j
- // << " , distance : " << desc_1[i].distance(desc_2[j]) << std::endl;
+ param.use_descriptors_ = true;
+ param.matcher_distance_th_ = 100;
+ param.matcher_ddesc_th_ = 100;
+ param.nms_radius_ = 0.1;
+ param.neigh_b_ = 0.07;
+ param.b_ratio_ = 2.5;
+
+ laser_params.angle_step_ = 0.00701248;
+
+ LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko_2(param);
+ auto new_scene_target =
+ std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2.extractScene(scan_target, laser_params));
+ auto new_scene_reference =
+ std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2.extractScene(scan_ref, laser_params));
+
+ std::cout << "keypoints target size : " << new_scene_target->keypoints_list_.size() << std::endl;
+ std::cout << "keypoints reference size : " << new_scene_reference->keypoints_list_.size() << std::endl;
+
+ auto match_r_t = loop_cl_falko_2.matchScene(new_scene_reference, new_scene_target);
+ for (int i = 0; i < match_r_t->associations.size(); i++)
+ if (match_r_t->associations[i].second != -1)
+ {
+ std::cout << "id first : " << match_r_t->associations[i].first << std::endl;
+ std::cout << "id second : " << match_r_t->associations[i].second << std::endl;
+ }
+
+ auto key_ref = new_scene_reference->keypoints_list_;
+ auto key_target = new_scene_target->keypoints_list_;
+
+ std::vector<double> x_ref_all, x_target_all;
+ std::vector<double> y_ref_all, y_target_all;
+
+ // Plotting keypoints scenes
+ for (int i = 0; i < key_ref.size(); i++)
+ {
+ x_ref_all.push_back(key_ref[i].point.x());
+ y_ref_all.push_back(key_ref[i].point.y());
+ }
- // acum_distance += desc_1[i].distance(desc_2[j]);
+ for (int i = 0; i < key_target.size(); i++)
+ {
+ x_target_all.push_back(key_target[i].point.x());
+ y_target_all.push_back(key_target[i].point.y());
+ }
- // // grid to x and y
- // std::vector<int> x_pos, x_pos_rotated;
- // std::vector<int> y_pos, y_pos_rotated;
+ plt::title("AHTMatcher BSC without descriptors");
+ plt::plot(x_ref_all, y_ref_all, "ob");
+ plt::plot(x_target_all, y_target_all, "or");
- // int desc_1_number = i;
- // int desc_2_number = j;
- // auto grid_1 = new_scene->descriptors_list_rotated[i].grid;
- // auto grid_2 = new_scene_2->descriptors_list_rotated[j].grid;
+ std::vector<double> x_ref, x_target;
+ std::vector<double> y_ref, y_target;
+ for (auto asso : match_r_t->associations)
+ if (asso.second != -1)
+ {
+ // auto a = key_ref[asso.first].point.x();
+ x_ref.push_back(key_ref[asso.first].point.x());
+ y_ref.push_back(key_ref[asso.first].point.y());
- // for (int i = 0; i < radialRingNumber; i++)
- // for (int j = 0; j < circularSectorNumber; j++)
- // {
- // if (grid_1[i][j] > 0)
- // {
- // x_pos.push_back(i);
- // y_pos.push_back(j);
- // }
+ x_target.push_back(key_target[asso.second].point.x());
+ y_target.push_back(key_target[asso.second].point.y());
+ }
- // if (grid_2[i][j] > 0)
- // {
- // x_pos_rotated.push_back(i);
- // y_pos_rotated.push_back(j);
- // }
- // }
+ for (int i = 0; i < x_ref.size(); i++)
+ {
+ plt::plot({x_ref[i], x_target[i]}, {y_ref[i], y_target[i]}, "g");
+ }
- // // Plotting descriptors
- // plt::title("NNMatcher BSC only keypoints");
- // plt::subplot(2, 1, 1);
- // plt::xlabel(" descriprtor not rotated");
- // plt::plot(x_pos, y_pos, "ob");
-
- // plt::subplot(2, 1, 2);
- // plt::xlabel(" descriprtor rotated");
- // plt::plot(x_pos_rotated, y_pos_rotated, "or");
- // // plt::show();
- // }
- // }
- // }
- // // std::cout << "acum_distance : " << acum_distance << std::endl;
- // ASSERT_EQ(acum_distance, 39);
+ plt::show();
}
int main(int argc, char **argv)
--
GitLab