diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h index 1e862fdd1c620a28c4309c97781483abdaa268d0..991104584b47d85304f78f508b5cba4f6cb60a45 100644 --- a/src/loop_closure_falko.h +++ b/src/loop_closure_falko.h @@ -74,17 +74,17 @@ struct ParameterLoopClosureFalko }; /** \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 - * + * * It extracts scenes from a laserscanutils::LaserScan. The scenes contain keypoints and descriptors - * + * * It matches a target scene against a list of reference scenes. - * + * * 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. - * + * * \tparam D Descriptor type. <bsc> or <cgh> * \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor> * \tparam M Matcher type <nn_matcher> or <aht_matcher> @@ -141,6 +141,87 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract extract(*scan_falko, (new_scene->keypoints_list_)); // Compute descriptors 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; } @@ -205,6 +286,24 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract 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); + } }; } /* namespace laserscanutils */ diff --git a/src/scene_base.h b/src/scene_base.h index a7531625cdc7bb31f5d683eabf7219d33b9090ee..76cfa61df35fdfc628dd3f574114dd9b525ebfdf 100644 --- a/src/scene_base.h +++ b/src/scene_base.h @@ -16,6 +16,12 @@ namespace laserscanutils { struct SceneBase { int id; + double area_; + double perimeter_; + double max_distance_; + double mean_distance_; + Eigen::Vector2d mid_point_; + }; typedef std::shared_ptr<SceneBase> sceneBasePtr;