diff --git a/src/laser_scan.cpp b/src/laser_scan.cpp
index 3747de309ea2c1b26b6f7fbe8894217fb2d26991..398e859f9437d3244c48bf3817eb115836dc7f88 100644
--- a/src/laser_scan.cpp
+++ b/src/laser_scan.cpp
@@ -102,14 +102,13 @@ void LaserScan::ranges2xy(const LaserScanParams& _scan_params, Eigen::Matrix4s _
{
//check raw range integrity
//invalid range
- if (!isValidRange(ranges_raw_[ii], _scan_params))
+ if (isValidRange(ranges_raw_[ii], _scan_params))
{
//set as valid range
ranges_[ii] = ranges_raw_[ii];
//transform the laser hit from polar to 3D euclidean homogeneous
point_laser << ranges_[ii] * cos(azimuth), ranges_[ii] * sin(azimuth), 0, 1;
-
//apply device mounting point calibration (p_r = T * p_l)
point_ref = _device_T * point_laser;
diff --git a/src/loop_closure_falko.h b/src/loop_closure_falko.h
index a4a534390cacef23b5ada68df2369d17d36a7ef0..127b392eec43917ba49d877b6ec41c5df0f04136 100644
--- a/src/loop_closure_falko.h
+++ b/src/loop_closure_falko.h
@@ -135,14 +135,14 @@ 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 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
+ // Compute KEYPOINTS 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++)
@@ -154,7 +154,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
new_scene->max_distance_ = distance;
}
- // discard too close by kp
+ // discard too close by KEYPOINTS
for (int i = 0; i < keypoints_list.size(); i++)
{
int repeated = 0;
@@ -163,7 +163,7 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
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)
+ if (distance < 0.03)
{
repeated = 1;
}
@@ -174,12 +174,12 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
}
}
- // Compute descriptors
+ // 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
+ // Compute KEYPOINTS 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++)
@@ -187,14 +187,13 @@ 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);
- // 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
+ // Compute KEYPOINTS Area and Perimeter
int n = 3;
double X[n];
double Y[n];
@@ -225,9 +224,6 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
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];
@@ -250,7 +246,7 @@ 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
+ // Compue KEYPOINTS 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++)
@@ -261,16 +257,10 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
(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
+ // rotate KEYPOINTS
for (int i = 0; i < new_scene->keypoints_list_.size(); i++)
{
falkolib::FALKO keypoint_mid;
@@ -301,6 +291,101 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
new_scene->keypoints_list_transl_rot_.push_back(keypoint_rot_trans);
}
+ // Compute KEYPOINTS covariance matrix
+
+ float sumXX = 0;
+ float sumXY = 0;
+ float sumYY = 0;
+
+ n = new_scene->keypoints_list_.size();
+ auto mean = new_scene->mid_point_;
+ for (int i = 0; i < n; i++)
+ {
+ sumXX += (new_scene->keypoints_list_[i].point[0] - mean[0]) *
+ (new_scene->keypoints_list_[i].point[0] - mean[0]);
+ sumXY += (new_scene->keypoints_list_[i].point[0] - mean[0]) *
+ (new_scene->keypoints_list_[i].point[1] - mean[1]);
+ sumYY += (new_scene->keypoints_list_[i].point[1] - mean[1]) *
+ (new_scene->keypoints_list_[i].point[1] - mean[1]);
+ }
+ double covXX = sumXX / n;
+ double covXY = sumXY / n;
+ double covYY = sumYY / n;
+
+ Eigen::Matrix<double, 2, 2> A;
+ A << covXX, covXY, covXY, covYY;
+
+ // Compute KEYPOINTS eigenvalues
+ Eigen::EigenSolver<Eigen::Matrix<double, 2, 2>> s(A);
+ double eig1 = s.eigenvalues()(0).real();
+ double eig2 = s.eigenvalues()(1).real();
+ new_scene->eigenvalues_kp_.push_back(eig1);
+ new_scene->eigenvalues_kp_.push_back(eig2);
+
+ // Compute SCAN mid point
+ LaserScan scan = _scan;
+ scan.ranges2xy(_scan_params);
+ Eigen::Vector2d mid_point_scan(0, 0);
+
+ for (int i = 0; i < scan.points_.size() / 3; i++)
+ {
+ mid_point_scan[0] = mid_point_scan[0] + scan.points_(0, i);
+ mid_point_scan[1] = mid_point_scan[1] + scan.points_(1, i);
+ }
+ new_scene->mid_point_scan_ = mid_point_scan / (double)scan.points_.size() / 3;
+
+ // Compute SCAN Area
+ n = 3;
+ new_scene->area_scan_ = 0.0;
+
+ int points_size = scan.points_.size() / 3;
+ for (int i = 0; i < points_size; i++)
+ {
+ X[0] = 0.0;
+ Y[0] = 0.0;
+
+ X[1] = scan.points_(0, i);
+ Y[1] = scan.points_(1, i);
+
+ if (i < points_size - 1) // Proceed until final keypoint
+ {
+ X[2] = scan.points_(0, i + 1);
+ Y[2] = scan.points_(1, i + 1);
+ }
+ else // if you arrived to the final keypoint then use inital keypoint
+ {
+ X[2] = scan.points_(0, 0);
+ Y[2] = scan.points_(1, 0);
+ }
+ new_scene->area_scan_ = new_scene->area_scan_ + (double)triangleArea(X, Y, n);
+ }
+ // Compute SCAN covariance matrix
+
+ sumXX = 0;
+ sumXY = 0;
+ sumYY = 0;
+
+ mean = new_scene->mid_point_scan_;
+ for (int i = 0; i < points_size; i++)
+ {
+ sumXX += (scan.points_(0, i) - mean[0]) * (scan.points_(0, i) - mean[0]);
+ sumXY += (scan.points_(0, i) - mean[0]) * (scan.points_(1, i) - mean[1]);
+ sumYY += (scan.points_(1, i) - mean[1]) * (scan.points_(1, i) - mean[1]);
+ }
+ covXX = sumXX / points_size;
+ covXY = sumXY / points_size;
+ covYY = sumYY / points_size;
+
+ Eigen::Matrix<double, 2, 2> A_scan;
+ A_scan << covXX, covXY, covXY, covYY;
+
+ // Compute SCAN eigenvalues
+ Eigen::EigenSolver<Eigen::Matrix<double, 2, 2>> s_scan(A_scan);
+ eig1 = s_scan.eigenvalues()(0).real();
+ eig2 = s_scan.eigenvalues()(1).real();
+ new_scene->eigenvalues_scan_.push_back(eig1);
+ new_scene->eigenvalues_scan_.push_back(eig2);
+
return new_scene;
}
@@ -309,10 +394,10 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
**/
laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params)
{
- double field_of_view = _scan_params.angle_max_-_scan_params.angle_min_;
- auto scan_falko = std::make_shared<falkolib::LaserScan>(_scan_params.angle_min_, field_of_view,
- _scan.ranges_raw_.size());
-
+ double field_of_view = _scan_params.angle_max_ - _scan_params.angle_min_;
+ auto scan_falko =
+ std::make_shared<falkolib::LaserScan>(_scan_params.angle_min_, field_of_view, _scan.ranges_raw_.size());
+
std::vector<double> double_ranges(_scan.ranges_raw_.begin(), _scan.ranges_raw_.end());
for (int i = 0; i < double_ranges.size(); i++)
diff --git a/src/scene_base.h b/src/scene_base.h
index 740abf3bd292512e81ec5752c07c02a666129f1d..40651e4f9146e19c52791a0069eb08a8cefedc70 100644
--- a/src/scene_base.h
+++ b/src/scene_base.h
@@ -15,14 +15,17 @@ namespace laserscanutils {
struct SceneBase
{
- int id;
- double area_;
- double perimeter_;
- double max_distance_;
- double mean_distance_;
- Eigen::Vector2d mid_point_;
+ int id;
+ double area_;
+ double area_scan_;
+ double perimeter_;
+ double max_distance_;
+ double mean_distance_;
+ Eigen::Vector2d mid_point_;
+ Eigen::Vector2d mid_point_scan_;
std::vector<double> regressor_coefs;
-
+ std::vector<double> eigenvalues_kp_;
+ std::vector<double> eigenvalues_scan_;
};
typedef std::shared_ptr<SceneBase> sceneBasePtr;
diff --git a/test/gtest_loop_closure_falko.cpp b/test/gtest_loop_closure_falko.cpp
index 57324387d8af3f2548ee1ae031c3c4a50e8ba875..eb0f9cf6c599b3577d0d48f2bf17adb8caa3d4cf 100644
--- a/test/gtest_loop_closure_falko.cpp
+++ b/test/gtest_loop_closure_falko.cpp
@@ -16,6 +16,8 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
LaserScanParams laser_params;
laser_params.angle_min_ = 0;
laser_params.angle_max_ = 2.0 * M_PI;
+ laser_params.angle_step_ = 0.00701248;
+ laser_params.range_max_ = 50;
for (int i = 0; i < scan_size; i++)
{
scan.ranges_raw_.push_back(testRanges1[i]);
@@ -33,10 +35,10 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
ASSERT_EQ(firstPoint, 250);
// Test extractScene2
- auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan, laser_params));
- auto new_scene2 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan2, laser_params));
- int detectedKeypoints = new_scene->keypoints_list_.size();
- int detectedDescriptors = new_scene->descriptors_list_.size();
+ auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan,
+ laser_params)); auto new_scene2 =
+ std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan2, laser_params)); int detectedKeypoints
+ = new_scene->keypoints_list_.size(); int detectedDescriptors = new_scene->descriptors_list_.size();
ASSERT_EQ(detectedKeypoints, 18);
ASSERT_EQ(detectedDescriptors, 18);
@@ -68,6 +70,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
laser_params.angle_min_ = 0;
laser_params.angle_max_ = 2.0 * M_PI;
laser_params.angle_step_ = laser_params.angle_max_ / 1440;
+ laser_params.range_max_ = 50;
for (int i = 0; i < scan_size; i++)
{
@@ -112,7 +115,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
asso_number = j;
}
}
- 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++)
@@ -178,6 +181,7 @@ TEST(loop_closure_falko, TestMatch)
laser_params.angle_min_ = 0;
laser_params.angle_max_ = 2.0 * M_PI;
laser_params.angle_step_ = laser_params.angle_max_ / 1440;
+ laser_params.range_max_ = 50;
for (int i = 0; i < scan_size; i++)
{
@@ -219,9 +223,10 @@ TEST(loop_closure_falko, TestMatch2)
laser_params.angle_min_ = 0;
laser_params.angle_max_ = 2.0 * M_PI;
laser_params.angle_step_ = laser_params.angle_max_ / 1440;
+ laser_params.range_max_ = 100;
// ** TEST WITH TARGET AND REFERENCE SCENE
- std::cout << "scan size : " << target_scan_1.size() << std::endl;
+ // std::cout << "scan size : " << target_scan_1.size() << std::endl;
for (int i = 0; i < target_scan_1.size(); i++)
{
scan_target.ranges_raw_.push_back(target_scan_1[i]);
@@ -236,7 +241,6 @@ TEST(loop_closure_falko, TestMatch2)
param.neigh_b_ = 0.1;
param.b_ratio_ = 4;
param.enable_subbeam_ = false;
- //
laser_params.angle_step_ = 0.00701248;
@@ -246,18 +250,18 @@ TEST(loop_closure_falko, TestMatch2)
auto new_scene_reference =
std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2.extractScene(scan_ref, laser_params));
- std::cout << "keypoints target size : " << new_scene_target->keypoints_list_.size() << std::endl;
- std::cout << "keypoints reference size : " << new_scene_reference->keypoints_list_.size() << std::endl;
+ // std::cout << "keypoints target size : " << new_scene_target->keypoints_list_.size() << std::endl;
+ // std::cout << "keypoints reference size : " << new_scene_reference->keypoints_list_.size() << std::endl;
auto match_r_t = loop_cl_falko_2.matchScene(new_scene_reference, new_scene_target);
for (int i = 0; i < match_r_t->associations.size(); i++)
if (match_r_t->associations[i].second != -1)
{
- std::cout << "id first : " << match_r_t->associations[i].first << std::endl;
- std::cout << "id second : " << match_r_t->associations[i].second << std::endl;
+ // std::cout << "id first : " << match_r_t->associations[i].first << std::endl;
+ // std::cout << "id second : " << match_r_t->associations[i].second << std::endl;
}
- std::cout << "transform : " << match_r_t->transform_vector.transpose() << std::endl;
+ // std::cout << "transform : " << match_r_t->transform_vector.transpose() << std::endl;
auto key_ref = new_scene_reference->keypoints_list_;
auto key_target = new_scene_target->keypoints_list_;
@@ -300,7 +304,7 @@ TEST(loop_closure_falko, TestMatch2)
plt::plot({x_ref[i], x_target[i]}, {y_ref[i], y_target[i]}, "g");
}
- plt::show();
+ // plt::show();
}
int main(int argc, char **argv)