diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h
index c4f94ca178be25d09b6d1e6ea7f5cd3d26c17e15..26961e5ff081f6415602ce8a3128172bd8759492 100644
--- a/src/loop_closure_falko.h
+++ b/src/loop_closure_falko.h
@@ -63,14 +63,13 @@ struct ParameterLoopClosureFalko
// matching
double matcher_ddesc_th_ = 0.2;
-
// aht matcher
- double xRes_ = 0.1;
- double yRes_ = 0.1;
- double thetaRes_ = 0.04;
- double xAbsMax_ = 5;
- double yAbsMax_ = 5;
- double thetaAbsMax_ = 1.57;
+ double xRes_ = 0.15;
+ double yRes_ = 0.15;
+ double thetaRes_ = 0.1;
+ double xAbsMax_ = 3;
+ double yAbsMax_ = 3;
+ double thetaAbsMax_ = 3;
};
/** \brief A class for loop closure using falko library
@@ -94,7 +93,6 @@ struct ParameterLoopClosureFalko
template <typename D, typename Extr, template <typename, typename> typename M>
class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtractor
{
- private:
public:
typedef std::shared_ptr<SceneFalko<D>> sceneFalkoBSCPtr;
typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr;
@@ -127,8 +125,311 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
use_descriptors_ = _param.use_descriptors_;
};
- // Template specialization
+ /** \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;
+
+ // 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
+ }
+
+ // Return absolute value
+ 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
+{
+ 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() {}
@@ -191,16 +492,9 @@ 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];
angle_rotation.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index);
- // 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]);
}
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index 878ac4bf6c4d7871eb825123a8f59949ab691587..6394926a1b4982807b38f0495b62daa86d4f379a 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -88,7 +88,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
auto new_scene_2 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_2, laser_params));
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_1 = new_scene->descriptors_list_;
auto desc_2 = new_scene_2->descriptors_list_rotated;
int radialRingNumber = new_scene->descriptors_list_[0].radialRingNumber;
@@ -111,9 +111,8 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
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 << "pair : " << i << " , " << asso_number << " , distance : " << min_dist << std::endl;
}
// for (int i = 0; i < desc_1.size(); i++)
@@ -167,7 +166,6 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
// }
// }
// }
- std::cout << "acum_distance : " << acum_distance << std::endl;
ASSERT_EQ(acum_distance, 39);
}