Skip to content
Snippets Groups Projects
Commit b731ea4f authored by Sergi Pujol Badell's avatar Sergi Pujol Badell
Browse files

added gtest for descriptors rotation

parent de94fb40
No related branches found
No related tags found
1 merge request!4Resolve "Implementation of Falko lib"
Expand all Hide whitespace changes
Inline Side-by-side
src/loop_closure_falko.h
+ 30
− 19
View file @ b731ea4f
...@@ -143,7 +143,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -143,7 +143,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
std::vector<falkolib::FALKO> keypoints_list; std::vector<falkolib::FALKO> keypoints_list;
extract(*scan_falko, keypoints_list); extract(*scan_falko, keypoints_list);
// Compute angle vector for keypoints
double angle_min = _scan_params.angle_min_; double angle_min = _scan_params.angle_min_;
double angle_step = _scan_params.angle_step_; double angle_step = _scan_params.angle_step_;
...@@ -181,20 +180,26 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -181,20 +180,26 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
// Compute descriptors // Compute descriptors
extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_);
std::vector<D> descriptors_list_rotated;
extractor_.compute(*scan_falko, new_scene->keypoints_list_, descriptors_list_rotated);
// Compute Scene mid point, angle for each keypoint and rotate descriptors // Compute Scene mid point, angle for each keypoint and rotate descriptors
Eigen::Vector2d mid_point(0, 0); Eigen::Vector2d mid_point(0, 0);
new_scene->descriptors_list_rotated = new_scene->descriptors_list_; std::vector<double> angle_rotation;
for (int i = 0; i < new_scene->keypoints_list_.size(); i++) for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
{ {
mid_point[0] = mid_point[0] + new_scene->keypoints_list_[i].point[0]; 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]; 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); // new_scene->angle_rotation_.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index);
new_scene->descriptors_list_rotated[i].rotate(-new_scene->angle_rotation_[i]); 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]);
new_scene->descriptors_list_rotated.push_back(descriptors_list_rotated[i]);
} }
new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size(); new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size();
// Compute Scene Area and Perimeter // Compute Scene Area and Perimeter
int n = 3; int n = 3;
double X[n]; double X[n];
...@@ -252,7 +257,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -252,7 +257,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
} }
// Compue Scene linear regresion and initial angle // Compue Scene linear regresion and initial angle
double ss_xy = 0; double ss_xy = 0;
double ss_xx = 0; double ss_xx = 0;
for (int i = 0; i <= new_scene->keypoints_list_.size(); i++) for (int i = 0; i <= new_scene->keypoints_list_.size(); i++)
...@@ -263,10 +267,10 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -263,10 +267,10 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
(new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]); (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]);
} }
double b_1 = ss_xy / ss_xx; double b_1 = ss_xy / ss_xx;
double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0]; // double b_0 = new_scene->mid_point_[1] - b_1 * new_scene->mid_point_[0];
new_scene->regressor_coefs.push_back(b_0); // new_scene->regressor_coefs.push_back(b_0);
new_scene->regressor_coefs.push_back(b_1); // new_scene->regressor_coefs.push_back(b_1);
double initial_angle = -atan(b_1); double initial_angle = -atan(b_1);
...@@ -280,7 +284,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -280,7 +284,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
keypoint_mid.point[0] = new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]; keypoint_mid.point[0] = new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0];
keypoint_mid.point[1] = new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]; keypoint_mid.point[1] = new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1];
new_scene->keipoints_list_mid_point_.push_back(keypoint_mid); new_scene->keypoints_list_mid_point_.push_back(keypoint_mid);
falkolib::FALKO keypoint_rotated; falkolib::FALKO keypoint_rotated;
keypoint_rotated = new_scene->keypoints_list_[i]; keypoint_rotated = new_scene->keypoints_list_[i];
...@@ -288,7 +292,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -288,7 +292,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
new_scene->keypoints_list_[i].point[1] * sin(initial_angle); new_scene->keypoints_list_[i].point[1] * sin(initial_angle);
keypoint_rotated.point[1] = new_scene->keypoints_list_[i].point[0] * sin(initial_angle) + keypoint_rotated.point[1] = new_scene->keypoints_list_[i].point[0] * sin(initial_angle) +
new_scene->keypoints_list_[i].point[1] * cos(initial_angle); new_scene->keypoints_list_[i].point[1] * cos(initial_angle);
new_scene->keipoints_list_rotated_.push_back(keypoint_rotated); new_scene->keypoints_list_rotated_.push_back(keypoint_rotated);
falkolib::FALKO keypoint_rot_trans; falkolib::FALKO keypoint_rot_trans;
keypoint_rot_trans = new_scene->keypoints_list_[i]; keypoint_rot_trans = new_scene->keypoints_list_[i];
...@@ -300,7 +304,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -300,7 +304,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
(new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * sin(initial_angle) + (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * sin(initial_angle) +
(new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * cos(initial_angle); (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * cos(initial_angle);
new_scene->keipoints_list_transl_rot_.push_back(keypoint_rot_trans); new_scene->keypoints_list_transl_rot_.push_back(keypoint_rot_trans);
} }
return new_scene; return new_scene;
...@@ -332,14 +336,21 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -332,14 +336,21 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
if (use_descriptors_ == 0) if (use_descriptors_ == 0)
{ {
matching_number = matcher_.match(scene_1_falko->keipoints_list_transl_rot_, // matching_number = matcher_.match(scene_1_falko->keypoints_list_transl_rot_,
scene_2_falko->keipoints_list_transl_rot_, asso_nn); // scene_2_falko->keypoints_list_transl_rot_, asso_nn);
matching_number =
matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn);
} }
else if (use_descriptors_ == 1) else if (use_descriptors_ == 1)
{ {
matching_number = matcher_.match( // matching_number = matcher_.match(
scene_1_falko->keipoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated, // scene_1_falko->keypoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated,
scene_2_falko->keipoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn); // scene_2_falko->keypoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn);
matching_number =
matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_rotated,
scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_rotated, asso_nn);
} }
auto new_match = std::make_shared<MatchLoopClosureScene>(); auto new_match = std::make_shared<MatchLoopClosureScene>();
new_match->keypoints_number_match = matching_number; new_match->keypoints_number_match = matching_number;
......
src/scene_falko.h
+ 4
− 4
View file @ b731ea4f
...@@ -35,10 +35,10 @@ typedef falkolib::CGH cgh; ...@@ -35,10 +35,10 @@ typedef falkolib::CGH cgh;
template <typename D> struct SceneFalko : public SceneBase template <typename D> struct SceneFalko : public SceneBase
{ {
std::vector<falkolib::FALKO> keypoints_list_; std::vector<falkolib::FALKO> keypoints_list_;
std::vector<falkolib::FALKO> keipoints_list_mid_point_; std::vector<falkolib::FALKO> keypoints_list_mid_point_;
std::vector<falkolib::FALKO> keipoints_list_transl_rot_; std::vector<falkolib::FALKO> keypoints_list_transl_rot_;
std::vector<falkolib::FALKO> keipoints_list_rotated_; std::vector<falkolib::FALKO> keypoints_list_rotated_;
std::vector<falkolib::FALKO> keipoints_list_rotated_reverse_; std::vector<falkolib::FALKO> keypoints_list_rotated_reverse_;
std::vector<D> descriptors_list_; std::vector<D> descriptors_list_;
std::vector<D> descriptors_list_rotated; std::vector<D> descriptors_list_rotated;
std::vector<double> angle_rotation_; std::vector<double> angle_rotation_;
......
test/CMakeLists.txt
+ 5
− 3
View file @ b731ea4f
...@@ -6,8 +6,9 @@ include_directories(${GTEST_INCLUDE_DIRS}) ...@@ -6,8 +6,9 @@ include_directories(${GTEST_INCLUDE_DIRS})
#Include directories #Include directories
INCLUDE_DIRECTORIES(../src) INCLUDE_DIRECTORIES(../src)
INCLUDE_DIRECTORIES(../test) INCLUDE_DIRECTORIES(/data)
FIND_PACKAGE(Eigen3 3.3 REQUIRED) FIND_PACKAGE(Eigen3 3.3 REQUIRED)
find_package(PythonLibs 2.7)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS}) INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
############# USE THIS TEST AS AN EXAMPLE #################### ############# USE THIS TEST AS AN EXAMPLE ####################
...@@ -22,5 +23,6 @@ INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS}) ...@@ -22,5 +23,6 @@ INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
gnss_utils_add_gtest(gtest_example gtest_example.cpp) gnss_utils_add_gtest(gtest_example gtest_example.cpp)
target_link_libraries(gtest_example ${PROJECT_NAME}) target_link_libraries(gtest_example ${PROJECT_NAME})
gnss_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp ${PROJECT_SOURCE_DIR}/test/testData2.cpp) gnss_utils_add_gtest(gtest_loop_closure_falko gtest_loop_closure_falko.cpp ${PROJECT_SOURCE_DIR}/test/data)
target_link_libraries(gtest_loop_closure_falko ${PROJECT_NAME}) target_link_libraries(gtest_loop_closure_falko ${PROJECT_NAME} ${PYTHON_LIBRARIES})
target_include_directories(gtest_loop_closure_falko PRIVATE ${PYTHON_INCLUDE_DIRS})
test/data/scan_data.h 0 → 100644
+ 2903
− 0
View file @ b731ea4f
This diff is collapsed.
test/gtest_loop_closure_falko.cpp
+ 213
− 5
View file @ b731ea4f
#include "../src/loop_closure_base.h" #include "../src/loop_closure_base.h"
#include "../src/loop_closure_falko.h" #include "../src/loop_closure_falko.h"
#include "testData2.h" // #include "testData2.h"
#include "data/scan_data.h"
#include "gtest/utils_gtest.h" #include "gtest/utils_gtest.h"
#include "matplotlibcpp.h"
using namespace laserscanutils; using namespace laserscanutils;
namespace plt = matplotlibcpp;
TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
{ {
...@@ -19,8 +22,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ...@@ -19,8 +22,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
scan2.ranges_raw_.push_back(testRanges2[i]); scan2.ranges_raw_.push_back(testRanges2[i]);
} }
ParameterLoopClosureFalko param; ParameterLoopClosureFalko param;
param.matcher_distance_th_= 0.3; param.matcher_distance_th_ = 0.3;
LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param); LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
// Test convert2LaserScanFALKO // Test convert2LaserScanFALKO
...@@ -37,7 +40,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ...@@ -37,7 +40,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
ASSERT_EQ(detectedKeypoints, 18); ASSERT_EQ(detectedKeypoints, 18);
ASSERT_EQ(detectedDescriptors, 18); ASSERT_EQ(detectedDescriptors, 18);
// Test matcheScene // Test matchScene
auto new_match = loop_cl_falko.matchScene(new_scene, new_scene); auto new_match = loop_cl_falko.matchScene(new_scene, new_scene);
ASSERT_EQ(new_match->keypoints_number_match, 18); ASSERT_EQ(new_match->keypoints_number_match, 18);
...@@ -52,7 +55,212 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions) ...@@ -52,7 +55,212 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
ASSERT_EQ(best_match->match, true); ASSERT_EQ(best_match->match, true);
ASSERT_EQ(best_match->scene_1, new_scene); ASSERT_EQ(best_match->scene_1, new_scene);
ASSERT_EQ(best_match->scene_2, new_scene); ASSERT_EQ(best_match->scene_2, new_scene);
ASSERT_EQ(best_match->score, 1); ASSERT_EQ(best_match->score, 1.8);
}
TEST(loop_closure_falko, TestDescriptorsRotation)
{
// Initialization
int scan_size = 1440;
LaserScan scan_1, scan2, scan_2;
LaserScanParams laser_params;
laser_params.angle_min_ = 0;
laser_params.angle_max_ = 2.0 * M_PI;
laser_params.angle_step_ = laser_params.angle_max_ / 1440;
for (int i = 0; i < scan_size; i++)
{
scan_1.ranges_raw_.push_back(testRanges1[i]);
scan_2.ranges_raw_.push_back(testRanges1[i]);
scan2.ranges_raw_.push_back(testRanges2[i]);
}
// Rotate scans
int rot = 800;
std::rotate(scan_2.ranges_raw_.begin(), scan_2.ranges_raw_.begin() + rot, scan_2.ranges_raw_.end());
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 0.3;
LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
// Test extractScene
auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_1, laser_params));
auto new_scene_2 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_2, laser_params));
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;
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);
}
TEST(loop_closure_falko, TestMatch)
{
// Initialization
int scan_size = 1440;
LaserScan scan_1, scan_2, scan_3;
LaserScanParams laser_params;
laser_params.angle_min_ = 0;
laser_params.angle_max_ = 2.0 * M_PI;
laser_params.angle_step_ = laser_params.angle_max_ / 1440;
for (int i = 0; i < scan_size; i++)
{
scan_1.ranges_raw_.push_back(testRanges1[i]);
scan_2.ranges_raw_.push_back(testRanges1[i]);
scan_3.ranges_raw_.push_back(testRanges2[i]);
}
// Rotate scans
int rot = 800;
std::rotate(scan_2.ranges_raw_.begin(), scan_2.ranges_raw_.begin() + rot, scan_2.ranges_raw_.end());
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 100;
param.matcher_ddesc_th_ = 5;
LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
// Test extractScene
auto new_scene_1 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_1, laser_params));
auto new_scene_2 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_2, laser_params));
auto new_scene_3 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_3, laser_params));
auto match_1_2 = loop_cl_falko.matchScene(new_scene_1, new_scene_2);
std::vector<std::pair<int, int>> asso_1_2;
for (auto asso : match_1_2->associations)
if (asso.second != -1)
asso_1_2.push_back(asso);
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++)
{
std::cout << "id first : " << match_1_2->associations[i].first << std::endl;
std::cout << "id second : " << match_1_2->associations[i].second << std::endl;
}
// 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;
// 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);
} }
int main(int argc, char **argv) int main(int argc, char **argv)
......
test/matplotlibcpp.h 0 → 100644
+ 2555
− 0
View file @ b731ea4f
This diff is collapsed.
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment