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

wip

parent 97156f79
No related branches found
No related tags found
1 merge request!4Resolve "Implementation of Falko lib"
Hide whitespace changes
Inline Side-by-side
falkolib @ 8f341e9f
Compare 0017c6c5...8f341e9f
Subproject commit 0017c6c5be963b923684bfd8eb6bd8b7641ba1b6
Subproject commit 8f341e9f1739c445cd8352586d514e2e49e25718
src/loop_closure_falko.h
+ 12
− 7
View file @ 386c6f8f
......@@ -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();
......
test/gtest_loop_closure_falko.cpp
+ 135
− 108
View file @ 386c6f8f
......@@ -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)
......
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