Skip to content
Snippets Groups Projects
Commit 5c1195bc authored by Sergi Pujol Badell's avatar Sergi Pujol Badell
Browse files

computed max_dist between keypoints of an scene

parent c5e90bda
No related branches found
No related tags found
1 merge request!4Resolve "Implementation of Falko lib"
...@@ -74,17 +74,17 @@ struct ParameterLoopClosureFalko ...@@ -74,17 +74,17 @@ struct ParameterLoopClosureFalko
}; };
/** \brief A class for loop closure using falko library /** \brief A class for loop closure using falko library
* *
* The class is a wrapper of the falkolib that is designed to be used for loop closures in the wolf problem * The class is a wrapper of the falkolib that is designed to be used for loop closures in the wolf problem
* *
* It extracts scenes from a laserscanutils::LaserScan. The scenes contain keypoints and descriptors * It extracts scenes from a laserscanutils::LaserScan. The scenes contain keypoints and descriptors
* *
* It matches a target scene against a list of reference scenes. * It matches a target scene against a list of reference scenes.
* *
* The reference scenes are found from a search of the previous captures * The reference scenes are found from a search of the previous captures
* *
* Diferent types of descriptors can be used, and are specified as template parameters. * Diferent types of descriptors can be used, and are specified as template parameters.
* *
* \tparam D Descriptor type. <bsc> or <cgh> * \tparam D Descriptor type. <bsc> or <cgh>
* \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor> * \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor>
* \tparam M Matcher type <nn_matcher> or <aht_matcher> * \tparam M Matcher type <nn_matcher> or <aht_matcher>
...@@ -141,6 +141,87 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -141,6 +141,87 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
extract(*scan_falko, (new_scene->keypoints_list_)); extract(*scan_falko, (new_scene->keypoints_list_));
// Compute descriptors // Compute descriptors
extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_); extractor_.compute(*scan_falko, new_scene->keypoints_list_, new_scene->descriptors_list_);
// Compute Scene mid point
Eigen::Vector2d mid_point(0, 0);
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->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];
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 // 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]);
}
return new_scene; return new_scene;
} }
...@@ -205,6 +286,24 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract ...@@ -205,6 +286,24 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
int keypoints_number_th_; int keypoints_number_th_;
bool use_descriptors_; 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);
}
}; };
} /* namespace laserscanutils */ } /* namespace laserscanutils */
......
...@@ -16,6 +16,12 @@ namespace laserscanutils { ...@@ -16,6 +16,12 @@ namespace laserscanutils {
struct SceneBase struct SceneBase
{ {
int id; int id;
double area_;
double perimeter_;
double max_distance_;
double mean_distance_;
Eigen::Vector2d mid_point_;
}; };
typedef std::shared_ptr<SceneBase> sceneBasePtr; typedef std::shared_ptr<SceneBase> sceneBasePtr;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment