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

added scan area and covariance extraction. Solved bug in LaserScan::getXY

parent b8c704f8
No related branches found
No related tags found
1 merge request!4Resolve "Implementation of Falko lib"
Hide whitespace changes
Inline Side-by-side
src/laser_scan.cpp
+ 1
− 2
View file @ 04945cc9
......@@ -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;
......
src/loop_closure_falko.h
+ 108
− 23
View file @ 04945cc9
......@@ -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++)
......
src/scene_base.h
+ 10
− 7
View file @ 04945cc9
......@@ -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;
......
test/gtest_loop_closure_falko.cpp
+ 17
− 13
View file @ 04945cc9
......@@ -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)
......
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment