Skip to content
Snippets Groups Projects
Commit c6af1e3f authored by Joan Vallvé Navarro's avatar Joan Vallvé Navarro
Browse files

removed plot dependency

parent 3c650c06
No related branches found
No related tags found
No related merge requests found
Expand all Hide whitespace changes
Inline Side-by-side
test/CMakeLists.txt
+ 1
− 3
View file @ c6af1e3f
......@@ -8,7 +8,6 @@ include_directories(${GTEST_INCLUDE_DIRS})
INCLUDE_DIRECTORIES(../src)
INCLUDE_DIRECTORIES(/data)
FIND_PACKAGE(Eigen3 3.3 REQUIRED)
find_package(PythonLibs 2.7)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIRS})
############# USE THIS TEST AS AN EXAMPLE ####################
......@@ -24,5 +23,4 @@ gnss_utils_add_gtest(gtest_example gtest_example.cpp)
target_link_libraries(gtest_example ${PROJECT_NAME})
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} ${PYTHON_LIBRARIES})
target_include_directories(gtest_loop_closure_falko PRIVATE ${PYTHON_INCLUDE_DIRS})
target_link_libraries(gtest_loop_closure_falko ${PROJECT_NAME})
test/gtest_loop_closure_falko.cpp
+ 62
− 127
View file @ c6af1e3f
#include "../src/loop_closure_base.h"
#include "../src/loop_closure_falko.h"
// #include "testData2.h"
#include "loop_closure_base.h"
#include "loop_closure_falko.h"
#include "data/scan_data.h"
#include "gtest/utils_gtest.h"
#include "matplotlibcpp.h"
using namespace laserscanutils;
namespace plt = matplotlibcpp;
TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
{
......@@ -19,10 +16,10 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
laser_params.angle_step_ = 0.00701248;
laser_params.range_max_ = 50;
for (int i = 0; i < scan_size; i++)
{
scan.ranges_raw_.push_back(testRanges1[i]);
scan2.ranges_raw_.push_back(testRanges2[i]);
}
{
scan.ranges_raw_.push_back(testRanges1[i]);
scan2.ranges_raw_.push_back(testRanges2[i]);
}
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 0.3;
......@@ -30,15 +27,16 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
// Test convert2LaserScanFALKO
std::shared_ptr<falkolib::LaserScan> scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);
int firstPoint = scan_falko->ranges[0];
int firstPoint = scan_falko->ranges[0];
ASSERT_EQ(firstPoint, 250);
// 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(); int detectedDescriptors = new_scene->descriptors_list_.size();
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();
int detectedDescriptors = new_scene->descriptors_list_.size();
ASSERT_EQ(detectedKeypoints, 18);
ASSERT_EQ(detectedDescriptors, 18);
......@@ -72,11 +70,11 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
laser_params.range_max_ = 50;
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]);
}
{
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());
......@@ -100,74 +98,22 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
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++)
{
double min_dist = 1000;
int asso_number;
for (int j = 0; j < desc_2.size(); j++)
{
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 << " , " << asso_number << " , distance : " << min_dist << 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();
// }
// }
// }
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 << " , " << asso_number << " , distance : " << min_dist << std::endl;
}
}
TEST(loop_closure_falko, TestMatch)
......@@ -183,11 +129,11 @@ TEST(loop_closure_falko, TestMatch)
laser_params.range_max_ = 50;
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]);
}
{
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;
......@@ -205,6 +151,8 @@ TEST(loop_closure_falko, TestMatch)
auto match_1_2 = loop_cl_falko.matchScene(new_scene_1, new_scene_2);
ASSERT_TRUE(match_1_2->match);
std::vector<std::pair<int, int>> asso_1_2;
for (auto asso : match_1_2->associations)
if (asso.second != -1)
......@@ -227,10 +175,10 @@ TEST(loop_closure_falko, TestMatch2)
// ** 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++)
{
scan_target.ranges_raw_.push_back(target_scan_1[i]);
scan_ref.ranges_raw_.push_back(reference_scan_1[i]);
}
{
scan_target.ranges_raw_.push_back(target_scan_1[i]);
scan_ref.ranges_raw_.push_back(reference_scan_1[i]);
}
ParameterLoopClosureFalko param;
param.use_descriptors_ = true;
......@@ -244,10 +192,8 @@ TEST(loop_closure_falko, TestMatch2)
laser_params.angle_step_ = 0.00701248;
LoopClosureFalkoAht<bsc, bscExtractor> 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));
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;
......@@ -255,10 +201,10 @@ TEST(loop_closure_falko, TestMatch2)
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;
}
{
std::cout << "id first : " << match_r_t->associations[i].first << std::endl;
std::cout << "id second : " << match_r_t->associations[i].second << std::endl;
}
// std::cout << "transform : " << match_r_t->transform_vector.transpose() << std::endl;
......@@ -270,40 +216,29 @@ TEST(loop_closure_falko, TestMatch2)
// 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());
}
{
x_ref_all.push_back(key_ref[i].point.x());
y_ref_all.push_back(key_ref[i].point.y());
}
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());
}
plt::title("AHTMatcher BSC without descriptors");
plt::plot(x_ref_all, y_ref_all, "ob");
plt::plot(x_target_all, y_target_all, "or");
{
x_target_all.push_back(key_target[i].point.x());
y_target_all.push_back(key_target[i].point.y());
}
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());
x_target.push_back(key_target[asso.second].point.x());
y_target.push_back(key_target[asso.second].point.y());
}
for (int i = 0; i < x_ref.size(); i++)
{
plt::plot({x_ref[i], x_target[i]}, {y_ref[i], y_target[i]}, "g");
}
// 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());
// plt::show();
x_target.push_back(key_target[asso.second].point.x());
y_target.push_back(key_target[asso.second].point.y());
}
}
int main(int argc, char **argv)
......
test/matplotlibcpp.h deleted 100644 → 0
+ 0
− 2555
View file @ 3c650c06
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