diff --git a/src/loop_closure_base.h b/src/loop_closure_base.h index b6a815f3b4900087a033637c3c2efede824e6279..b9fcd991852aac7d21d984de1963ad6206600f23 100644 --- a/src/loop_closure_base.h +++ b/src/loop_closure_base.h @@ -54,6 +54,10 @@ class LoopClosureBase2d for (auto ref_scene : _l_scenes) { auto match = matchScene(ref_scene, _new_scene); + + while (matchings.find(match->score)!=matchings.end()) + match->score+=0.001; + matchings.emplace(match->score, match); } return matchings; diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 127b392eec43917ba49d877b6ec41c5df0f04136..23a5def98a07998a078f93673e58b559c91b02ba 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -319,11 +319,20 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract Eigen::EigenSolver<Eigen::Matrix<double, 2, 2>> s(A); double eig1 = s.eigenvalues()(0).real(); double eig2 = s.eigenvalues()(1).real(); - new_scene->eigenvalues_kp_.push_back(eig1); - new_scene->eigenvalues_kp_.push_back(eig2); + // The greater eigenvalue will be the first one + if (eig1 > eig2) + { + new_scene->eigenvalues_kp_.push_back(eig1); + new_scene->eigenvalues_kp_.push_back(eig2); + } + else + { + new_scene->eigenvalues_kp_.push_back(eig2); + new_scene->eigenvalues_kp_.push_back(eig1); + } // Compute SCAN mid point - LaserScan scan = _scan; + LaserScan scan = _scan; scan.ranges2xy(_scan_params); Eigen::Vector2d mid_point_scan(0, 0); @@ -383,8 +392,18 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract Eigen::EigenSolver<Eigen::Matrix<double, 2, 2>> s_scan(A_scan); eig1 = s_scan.eigenvalues()(0).real(); eig2 = s_scan.eigenvalues()(1).real(); - new_scene->eigenvalues_scan_.push_back(eig1); - new_scene->eigenvalues_scan_.push_back(eig2); + + // The greater eigenvalue will be the first one + if (eig1 > eig2) + { + new_scene->eigenvalues_scan_.push_back(eig1); + new_scene->eigenvalues_scan_.push_back(eig2); + } + else + { + new_scene->eigenvalues_scan_.push_back(eig2); + new_scene->eigenvalues_scan_.push_back(eig1); + } return new_scene; } @@ -426,48 +445,31 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract if (use_descriptors_ == 0) { - // matching_number = matcher_.match(scene_1_falko->keypoints_list_transl_rot_, - // 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) { - // matching_number = matcher_.match( - // scene_1_falko->keypoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated, - // 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); } - // std::cout << "laserscanutils.LoopClosureFalko.matchScene -> num of skp matched : " << matching_number << - // std::endl; - auto new_match = std::make_shared<MatchLoopClosureScene>(); new_match->keypoints_number_match = matching_number; - // if (matching_number >= keypoints_number_th_ - 2) - // { + new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn, new_match->transform); - // } - // else - // { - // new_match->match = false; - // } new_match->associations = asso_nn; new_match->scene_1 = _scene_1; new_match->scene_2 = _scene_2; - // new_match->score = (double)matching_number / (double)std::min(scene_1_falko->keypoints_list_.size(), - // scene_2_falko->keypoints_list_.size()); - new_match->transform_vector.head(2) = new_match->transform.translation(); new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle(); + std::cout << "laserscanutils::matching_number : " << matching_number << std::endl; + new_match->score = (double)matching_number; return new_match;