diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h
index 26961e5ff081f6415602ce8a3128172bd8759492..7bb20beb3b748c3b23ce2f1852ba0b28768b469d 100644
--- a/src/loop_closure_falko.h
+++ b/src/loop_closure_falko.h
@@ -103,11 +103,11 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
/** \brief Constructor
* \param _param parameter struct with falko lib parameters
**/
- LoopClosureFalko(ParameterLoopClosureFalko _param)
+ LoopClosureFalko(ParameterLoopClosureFalko _param, const M<falkolib::FALKO, D> &_matcher)
: LoopClosureBase2d()
, falkolib::FALKOExtractor()
, extractor_(_param.circularSectorNumber_, _param.radialRingNumber_)
- , matcher_()
+ , matcher_(_matcher)
{
// FALKO Extractor Parameters
setMinExtractionRange(_param.min_extraction_range_);
@@ -346,17 +346,21 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
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_)
- {
- new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_,
- asso_nn, new_match->transform);
- }
- else
- {
- new_match->match = false;
- }
+ // 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;
@@ -365,11 +369,11 @@ 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;
-
new_match->transform_vector.head(2) = new_match->transform.translation();
new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle();
+ new_match->score = (double)matching_number;
+
return new_match;
}
@@ -394,310 +398,26 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
return fabs((double)area / 2.0);
}
};
+
// Partial template specialization
-template <typename D, typename Extr>
-class LoopClosureFalko<D, Extr, falkolib::AHTMatcher> : public LoopClosureBase2d, public falkolib::FALKOExtractor
+
+template <typename D, typename Extr> class LoopClosureFalkoAht : public LoopClosureFalko<D, Extr, falkolib::AHTMatcher>
{
public:
- typedef std::shared_ptr<SceneFalko<D>> sceneFalkoBSCPtr;
- typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr;
-
- Extr extractor_;
- falkolib::AHTMatcher<falkolib::FALKO, D> matcher_;
-
- /** \brief Constructor
- * \param _param parameter struct with falko lib parameters
- **/
- LoopClosureFalko(ParameterLoopClosureFalko _param)
- : LoopClosureBase2d()
- , falkolib::FALKOExtractor()
- , extractor_(_param.circularSectorNumber_, _param.radialRingNumber_)
- , matcher_(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_, _param.yAbsMax_, _param.thetaAbsMax_)
-
- {
- // FALKO Extractor Parameters
- setMinExtractionRange(_param.min_extraction_range_);
- setMaxExtractionRange(_param.max_extraction_range_);
- enableSubbeam(_param.enable_subbeam_);
- setNMSRadius(_param.nms_radius_);
- setNeighB(_param.neigh_b_);
- setBRatio(_param.b_ratio_);
- setGridSectors(_param.grid_sectors_);
-
- // Matcher Extractor Parameters
- matcher_.setDistanceThreshold(_param.matcher_distance_th_);
- matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_);
- keypoints_number_th_ = _param.keypoints_number_th_;
- use_descriptors_ = _param.use_descriptors_;
- };
- /** \brief Destructor
- **/
- ~LoopClosureFalko() {}
-
- /** \brief Create and update the scene struct with keypoints and descriptors
- **/
- sceneBasePtr extractScene(const LaserScan &_scan, const LaserScanParams &_scan_params) override
- {
- auto new_scene = std::make_shared<SceneFalko<D>>();
- auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params);
- // Extract keypoints
- std::vector<falkolib::FALKO> keypoints_list;
- extract(*scan_falko, keypoints_list);
-
- double angle_min = _scan_params.angle_min_;
- double angle_step = _scan_params.angle_step_;
-
- // Compute max_dist
- new_scene->max_distance_ = 0;
- for (int i = 0; i < keypoints_list.size(); i++)
- for (int j = 0; j < keypoints_list.size(); j++)
- {
- double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]);
- double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]);
- double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist));
- if (distance > new_scene->max_distance_)
- new_scene->max_distance_ = distance;
- }
-
- // discard too close by kp
- for (int i = 0; i < keypoints_list.size(); i++)
- {
- int repeated = 0;
- for (int j = i + 1; j < keypoints_list.size(); j++)
- {
- double X_dist = fabs(keypoints_list[i].point[0] - keypoints_list[j].point[0]);
- double Y_dist = fabs(keypoints_list[i].point[1] - keypoints_list[j].point[1]);
- double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist));
- if (distance < 0.05)
- {
- repeated = 1;
- }
- }
- if (repeated == 0)
- {
- new_scene->keypoints_list_.push_back(keypoints_list[i]);
- }
- }
-
- // Compute descriptors
- 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
- Eigen::Vector2d mid_point(0, 0);
- std::vector<double> angle_rotation;
- 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[1] = mid_point[1] + new_scene->keypoints_list_[i].point[1];
- angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index);
- // double orientation = new_scene->keypoints_list_[i].orientation + angle_rotation[i];
- double orientation = angle_rotation[i];
- descriptors_list_rotated[i].rotate(orientation);
- 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;
- double X[n];
- double Y[n];
- new_scene->perimeter_ = 0.0;
- new_scene->area_ = 0.0;
- Eigen::Vector2d dist_between_two_kp;
-
- if (new_scene->keypoints_list_.size() < 3)
- return new_scene;
-
- if (new_scene->keypoints_list_.size() < 4)
- {
- X[0] = new_scene->keypoints_list_[0].point[0];
- Y[0] = new_scene->keypoints_list_[0].point[1];
-
- X[1] = new_scene->keypoints_list_[1].point[0];
- Y[1] = new_scene->keypoints_list_[1].point[1];
-
- X[2] = new_scene->keypoints_list_[2].point[0];
- Y[2] = new_scene->keypoints_list_[2].point[1];
-
- new_scene->area_ = new_scene->area_ + triangleArea(X, Y, n);
- return new_scene;
- }
-
- for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
- {
- X[0] = new_scene->mid_point_[0];
- Y[0] = new_scene->mid_point_[1];
-
- // X[0] = 0.0;
- // Y[0] = 0.0;
-
- X[1] = new_scene->keypoints_list_[i].point[0];
- Y[1] = new_scene->keypoints_list_[i].point[1];
-
- if (i < new_scene->keypoints_list_.size() - 1) // Proceed until final keypoint
- {
- X[2] = new_scene->keypoints_list_[i + 1].point[0];
- Y[2] = new_scene->keypoints_list_[i + 1].point[1];
-
- dist_between_two_kp =
- new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[i + 1].point;
- }
- else // if you arrived to the final keypoint then use inital keypoint
- {
- X[2] = new_scene->keypoints_list_[0].point[0];
- Y[2] = new_scene->keypoints_list_[0].point[1];
-
- dist_between_two_kp = new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[0].point;
- }
- new_scene->area_ = new_scene->area_ + (double)triangleArea(X, Y, n);
- new_scene->perimeter_ = new_scene->perimeter_ + hypot(dist_between_two_kp[0], dist_between_two_kp[1]);
- }
-
- // Compue Scene linear regresion and initial angle
- double ss_xy = 0;
- double ss_xx = 0;
- for (int i = 0; i <= new_scene->keypoints_list_.size(); i++)
- {
- ss_xy += (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) *
- (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]);
- ss_xx += (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_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_1);
-
- double initial_angle = -atan(b_1);
-
- // double inital_angle_inv = initial_angle - M_PI;
-
- // rotate keypoints
- for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
- {
- falkolib::FALKO keypoint_mid;
- keypoint_mid = new_scene->keypoints_list_[i];
-
- 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];
- new_scene->keypoints_list_mid_point_.push_back(keypoint_mid);
-
- falkolib::FALKO keypoint_rotated;
- keypoint_rotated = new_scene->keypoints_list_[i];
- keypoint_rotated.point[0] = new_scene->keypoints_list_[i].point[0] * cos(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) +
- new_scene->keypoints_list_[i].point[1] * cos(initial_angle);
- new_scene->keypoints_list_rotated_.push_back(keypoint_rotated);
-
- falkolib::FALKO keypoint_rot_trans;
- keypoint_rot_trans = new_scene->keypoints_list_[i];
- keypoint_rot_trans.point[0] =
- (new_scene->keypoints_list_[i].point[0] - new_scene->mid_point_[0]) * cos(initial_angle) -
- (new_scene->keypoints_list_[i].point[1] - new_scene->mid_point_[1]) * sin(initial_angle);
-
- keypoint_rot_trans.point[1] =
- (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_transl_rot_.push_back(keypoint_rot_trans);
- }
-
- return new_scene;
- }
-
- /** \brief Convert scans from laserscanutils::LaserScan to
- *falkolib::LaserScan object
- **/
- laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params)
- {
- auto scan_falko = std::make_shared<falkolib::LaserScan>(_scan_params.angle_min_, _scan_params.angle_max_,
- _scan.ranges_raw_.size());
- std::vector<double> double_ranges(_scan.ranges_raw_.begin(), _scan.ranges_raw_.end());
- scan_falko->fromRanges(double_ranges);
- return scan_falko;
- }
-
- /** \brief Create and update a matchLoopClosure struct with the info that is produced when matching two given scenes
- * \param _scene_1 reference scene struct
- * \param _scene_2 target scene struct
- **/
- MatchLoopClosureScenePtr matchScene(sceneBasePtr _scene_1, sceneBasePtr _scene_2) override
- {
- std::vector<std::pair<int, int>> asso_nn;
- auto scene_1_falko = std::static_pointer_cast<SceneFalko<D>>(_scene_1);
- auto scene_2_falko = std::static_pointer_cast<SceneFalko<D>>(_scene_2);
-
- int matching_number = 0;
-
- 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);
- }
- auto new_match = std::make_shared<MatchLoopClosureScene>();
- new_match->keypoints_number_match = matching_number;
- if (matching_number > keypoints_number_th_)
- {
- 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->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();
-
- return new_match;
- }
-
- int keypoints_number_th_;
- bool use_descriptors_;
-
- // (X[i], Y[i]) are coordinates of i'th point.
- double triangleArea(double X[], double Y[], int n)
- {
- // Initialize area
- double area = 0.0;
+ LoopClosureFalkoAht(ParameterLoopClosureFalko _param)
+ : LoopClosureFalko<D, Extr, falkolib::AHTMatcher>(
+ _param,
+ falkolib::AHTMatcher<falkolib::FALKO, D>(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_,
+ _param.yAbsMax_, _param.thetaAbsMax_)){};
+};
- // Calculate value of shoelace formula
- int j = n - 1;
- for (int i = 0; i < n; i++)
- {
- area += (X[j] + X[i]) * (Y[j] - Y[i]);
- j = i; // j is previous vertex to i
- }
+template <typename D, typename Extr> class LoopClosureFalkoNn : public LoopClosureFalko<D, Extr, falkolib::NNMatcher>
+{
+ public:
+ LoopClosureFalkoNn(ParameterLoopClosureFalko _param)
+ : LoopClosureFalko<D, Extr, falkolib::NNMatcher>(_param, falkolib::NNMatcher<falkolib::FALKO, D>()){
- // Return absolute value
- return fabs((double)area / 2.0);
- }
+ };
};
} /* namespace laserscanutils */
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index 6394926a1b4982807b38f0495b62daa86d4f379a..9c0caf4587a8e74ab5b428e82c4365ddaa20a304 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -24,7 +24,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 0.3;
- LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
+ LoopClosureFalkoAht<bsc, bscExtractor> loop_cl_falko(param);
// Test convert2LaserScanFALKO
std::shared_ptr<falkolib::LaserScan> scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);
@@ -81,7 +81,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 0.3;
- LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
+ LoopClosureFalkoAht<bsc, bscExtractor> loop_cl_falko(param);
// Test extractScene
auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_1, laser_params));
@@ -194,7 +194,7 @@ TEST(loop_closure_falko, TestMatch)
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 100;
param.matcher_ddesc_th_ = 5;
- LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
+ LoopClosureFalkoAht<bsc, bscExtractor> loop_cl_falko(param);
// Test extractScene
auto new_scene_1 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_1, laser_params));
@@ -227,7 +227,7 @@ TEST(loop_closure_falko, TestMatch)
laser_params.angle_step_ = 0.00701248;
- LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko_2(param);
+ 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 =