diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h
index 991104584b47d85304f78f508b5cba4f6cb60a45..588e5316bb235899d3ec84115c5921cc4844b030 100644
--- a/src/loop_closure_falko.h
+++ b/src/loop_closure_falko.h
@@ -127,6 +127,8 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
use_descriptors_ = _param.use_descriptors_;
};
+ // Template specialization
+
/** \brief Destructor
**/
~LoopClosureFalko() {}
@@ -138,34 +140,61 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
auto new_scene = std::make_shared<SceneFalko<D>>();
auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params);
// Extract keypoints
- extract(*scan_falko, (new_scene->keypoints_list_));
+ std::vector<falkolib::FALKO> keypoints_list;
+ extract(*scan_falko, keypoints_list);
+
+ // Compute angle vector for keypoints
+ 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_);
- // Compute Scene mid point
+ // Compute Scene mid point, angle for each keypoint and rotate descriptors
Eigen::Vector2d mid_point(0, 0);
+ new_scene->descriptors_list_rotated = new_scene->descriptors_list_;
+
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];
+ new_scene->angle_rotation_.push_back(angle_min + angle_step * new_scene->keypoints_list_[i].index);
+ new_scene->descriptors_list_rotated[i].rotate(-new_scene->angle_rotation_[i]);
}
new_scene->mid_point_ = mid_point / new_scene->keypoints_list_.size();
- // Compute max_dist
- new_scene->max_distance_ = 0;
- for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
- for (int j = 0; j < new_scene->keypoints_list_.size(); j++)
- {
- double X_dist =
- fabs(new_scene->keypoints_list_[i].point[0] - new_scene->keypoints_list_[j].point[0]);
- double Y_dist =
- fabs(new_scene->keypoints_list_[i].point[1] - new_scene->keypoints_list_[j].point[1]);
- double distance = sqrt((X_dist * X_dist) + (Y_dist * Y_dist));
- std::cout << "distance : " << distance << std::endl;
- if (distance > new_scene->max_distance_)
- new_scene->max_distance_ = distance;
- }
-
// Compute Scene Area and Perimeter
int n = 3;
double X[n];
@@ -211,7 +240,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
dist_between_two_kp =
new_scene->keypoints_list_[i].point - new_scene->keypoints_list_[i + 1].point;
}
- else // then use inital keypoint
+ 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];
@@ -222,6 +251,58 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
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->keipoints_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->keipoints_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->keipoints_list_transl_rot_.push_back(keypoint_rot_trans);
+ }
+
return new_scene;
}
@@ -243,7 +324,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
**/
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);
@@ -252,14 +332,14 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
if (use_descriptors_ == 0)
{
- matching_number =
- matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn);
+ matching_number = matcher_.match(scene_1_falko->keipoints_list_transl_rot_,
+ scene_2_falko->keipoints_list_transl_rot_, asso_nn);
}
else if (use_descriptors_ == 1)
{
- matching_number =
- matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_,
- scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_, asso_nn);
+ matching_number = matcher_.match(
+ scene_1_falko->keipoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated,
+ scene_2_falko->keipoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn);
}
auto new_match = std::make_shared<MatchLoopClosureScene>();
new_match->keypoints_number_match = matching_number;
@@ -272,11 +352,15 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
{
new_match->match = false;
}
- 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->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 / 10.0;
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/src/match_loop_closure_scene.h b/src/match_loop_closure_scene.h
index 423398f07248da90f0ef57a91a68d4d5fb57d0f4..a99dc0045e3304ca433526452590a3e102d06e68 100644
--- a/src/match_loop_closure_scene.h
+++ b/src/match_loop_closure_scene.h
@@ -24,6 +24,7 @@ struct MatchLoopClosureScene {
bool match;
int keypoints_number_match;
double score;
+ std::vector<std::pair<int, int>> associations;
Eigen::Affine2d transform;
Eigen::Vector3d transform_vector;
};
diff --git a/src/scene_base.h b/src/scene_base.h
index 76cfa61df35fdfc628dd3f574114dd9b525ebfdf..740abf3bd292512e81ec5752c07c02a666129f1d 100644
--- a/src/scene_base.h
+++ b/src/scene_base.h
@@ -21,6 +21,7 @@ struct SceneBase
double max_distance_;
double mean_distance_;
Eigen::Vector2d mid_point_;
+ std::vector<double> regressor_coefs;
};
typedef std::shared_ptr<SceneBase> sceneBasePtr;
diff --git a/src/scene_falko.h b/src/scene_falko.h
index 18529b178873c4e9bc17b3dde0c23ada344cada1..d8b4795ce09b0768f58b5ed95b3f1616767710ec 100644
--- a/src/scene_falko.h
+++ b/src/scene_falko.h
@@ -35,7 +35,13 @@ typedef falkolib::CGH cgh;
template <typename D> struct SceneFalko : public SceneBase
{
std::vector<falkolib::FALKO> keypoints_list_;
+ std::vector<falkolib::FALKO> keipoints_list_mid_point_;
+ std::vector<falkolib::FALKO> keipoints_list_transl_rot_;
+ std::vector<falkolib::FALKO> keipoints_list_rotated_;
+ std::vector<falkolib::FALKO> keipoints_list_rotated_reverse_;
std::vector<D> descriptors_list_;
+ std::vector<D> descriptors_list_rotated;
+ std::vector<double> angle_rotation_;
};
} /* namespace laserscanutils */
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index f01b38bf2bda32e0219447e79d4263a84dd84697..378ec134f526863be676aed8fd5dc544eb58acf6 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -20,7 +20,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
}
ParameterLoopClosureFalko param;
- LoopClosureFalko<bsc, bscExtractor, nn_matcher> loop_cl_falko(param);
+ param.matcher_distance_th_= 0.3;
+ LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
// Test convert2LaserScanFALKO
std::shared_ptr<falkolib::LaserScan> scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);