diff --git a/deps/falkolib b/deps/falkolib index 0017c6c5be963b923684bfd8eb6bd8b7641ba1b6..8f341e9f1739c445cd8352586d514e2e49e25718 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 453cdbec7a8fe50c4575d6012b5c54768918386f..c4f94ca178be25d09b6d1e6ea7f5cd3d26c17e15 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 c711ab9d06cbb4fb6b7857e8362ef98b89405fa3..878ac4bf6c4d7871eb825123a8f59949ab691587 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)