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

wip

parent bfb166dc
No related branches found
No related tags found
1 merge request!4Resolve "Implementation of Falko lib"
Hide whitespace changes
Inline Side-by-side
src/loop_closure_falko.h
+ 32
− 312
View file @ 31e28c22
......@@ -103,11 +103,11 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
/** \brief Constructor
* \param _param parameter struct with falko lib parameters
**/
LoopClosureFalko(ParameterLoopClosureFalko _param)
LoopClosureFalko(ParameterLoopClosureFalko _param, const M<falkolib::FALKO, D> &_matcher)
: LoopClosureBase2d()
, falkolib::FALKOExtractor()
, extractor_(_param.circularSectorNumber_, _param.radialRingNumber_)
, matcher_()
, matcher_(_matcher)
{
// FALKO Extractor Parameters
setMinExtractionRange(_param.min_extraction_range_);
......@@ -346,17 +346,21 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_rotated,
scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_rotated, asso_nn);
}
// std::cout << "laserscanutils.LoopClosureFalko.matchScene -> num of skp matched : " << matching_number <<
// std::endl;
auto new_match = std::make_shared<MatchLoopClosureScene>();
new_match->keypoints_number_match = matching_number;
if (matching_number > keypoints_number_th_)
{
new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_,
asso_nn, new_match->transform);
}
else
{
new_match->match = false;
}
// if (matching_number >= keypoints_number_th_ - 2)
// {
new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn,
new_match->transform);
// }
// else
// {
// new_match->match = false;
// }
new_match->associations = asso_nn;
new_match->scene_1 = _scene_1;
......@@ -365,11 +369,11 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
// 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;
new_match->transform_vector.head(2) = new_match->transform.translation();
new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle();
new_match->score = (double)matching_number;
return new_match;
}
......@@ -394,310 +398,26 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
return fabs((double)area / 2.0);
}
};
// Partial template specialization
template <typename D, typename Extr>
class LoopClosureFalko<D, Extr, falkolib::AHTMatcher> : public LoopClosureBase2d, public falkolib::FALKOExtractor
template <typename D, typename Extr> class LoopClosureFalkoAht : public LoopClosureFalko<D, Extr, falkolib::AHTMatcher>
{
public:
typedef std::shared_ptr<SceneFalko<D>> sceneFalkoBSCPtr;
typedef std::shared_ptr<falkolib::LaserScan> laserScanPtr;
Extr extractor_;
falkolib::AHTMatcher<falkolib::FALKO, D> matcher_;
/** \brief Constructor
* \param _param parameter struct with falko lib parameters
**/
LoopClosureFalko(ParameterLoopClosureFalko _param)
: LoopClosureBase2d()
, falkolib::FALKOExtractor()
, extractor_(_param.circularSectorNumber_, _param.radialRingNumber_)
, matcher_(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_, _param.yAbsMax_, _param.thetaAbsMax_)
{
// FALKO Extractor Parameters
setMinExtractionRange(_param.min_extraction_range_);
setMaxExtractionRange(_param.max_extraction_range_);
enableSubbeam(_param.enable_subbeam_);
setNMSRadius(_param.nms_radius_);
setNeighB(_param.neigh_b_);
setBRatio(_param.b_ratio_);
setGridSectors(_param.grid_sectors_);
// Matcher Extractor Parameters
matcher_.setDistanceThreshold(_param.matcher_distance_th_);
matcher_.setDescriptorThreshold(_param.matcher_ddesc_th_);
keypoints_number_th_ = _param.keypoints_number_th_;
use_descriptors_ = _param.use_descriptors_;
};
/** \brief Destructor
**/
~LoopClosureFalko() {}
/** \brief Create and update the scene struct with keypoints and descriptors
**/
sceneBasePtr extractScene(const LaserScan &_scan, const LaserScanParams &_scan_params) override
{
auto new_scene = std::make_shared<SceneFalko<D>>();
auto scan_falko = convert2LaserScanFALKO(_scan, _scan_params);
// 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
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_);
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
Eigen::Vector2d mid_point(0, 0);
std::vector<double> angle_rotation;
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];
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
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 // 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];
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]);
}
// 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->keypoints_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->keypoints_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->keypoints_list_transl_rot_.push_back(keypoint_rot_trans);
}
return new_scene;
}
/** \brief Convert scans from laserscanutils::LaserScan to
*falkolib::LaserScan object
**/
laserScanPtr convert2LaserScanFALKO(const LaserScan &_scan, const LaserScanParams &_scan_params)
{
auto scan_falko = std::make_shared<falkolib::LaserScan>(_scan_params.angle_min_, _scan_params.angle_max_,
_scan.ranges_raw_.size());
std::vector<double> double_ranges(_scan.ranges_raw_.begin(), _scan.ranges_raw_.end());
scan_falko->fromRanges(double_ranges);
return scan_falko;
}
/** \brief Create and update a matchLoopClosure struct with the info that is produced when matching two given scenes
* \param _scene_1 reference scene struct
* \param _scene_2 target scene struct
**/
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);
int matching_number = 0;
if (use_descriptors_ == 0)
{
// matching_number = matcher_.match(scene_1_falko->keypoints_list_transl_rot_,
// scene_2_falko->keypoints_list_transl_rot_, asso_nn);
matching_number =
matcher_.match(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_, asso_nn);
}
else if (use_descriptors_ == 1)
{
// matching_number = matcher_.match(
// scene_1_falko->keypoints_list_transl_rot_, scene_1_falko->descriptors_list_rotated,
// scene_2_falko->keypoints_list_transl_rot_, scene_2_falko->descriptors_list_rotated, asso_nn);
matching_number =
matcher_.match(scene_1_falko->keypoints_list_, scene_1_falko->descriptors_list_rotated,
scene_2_falko->keypoints_list_, scene_2_falko->descriptors_list_rotated, asso_nn);
}
auto new_match = std::make_shared<MatchLoopClosureScene>();
new_match->keypoints_number_match = matching_number;
if (matching_number > keypoints_number_th_)
{
new_match->match = computeTransform(scene_1_falko->keypoints_list_, scene_2_falko->keypoints_list_,
asso_nn, new_match->transform);
}
else
{
new_match->match = false;
}
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;
new_match->transform_vector.head(2) = new_match->transform.translation();
new_match->transform_vector(2) = Eigen::Rotation2Dd(new_match->transform.rotation()).angle();
return new_match;
}
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;
LoopClosureFalkoAht(ParameterLoopClosureFalko _param)
: LoopClosureFalko<D, Extr, falkolib::AHTMatcher>(
_param,
falkolib::AHTMatcher<falkolib::FALKO, D>(_param.xRes_, _param.yRes_, _param.thetaRes_, _param.xAbsMax_,
_param.yAbsMax_, _param.thetaAbsMax_)){};
};
// 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
}
template <typename D, typename Extr> class LoopClosureFalkoNn : public LoopClosureFalko<D, Extr, falkolib::NNMatcher>
{
public:
LoopClosureFalkoNn(ParameterLoopClosureFalko _param)
: LoopClosureFalko<D, Extr, falkolib::NNMatcher>(_param, falkolib::NNMatcher<falkolib::FALKO, D>()){
// Return absolute value
return fabs((double)area / 2.0);
}
};
};
} /* namespace laserscanutils */
......
test/gtest_loop_closure_falko.cpp
+ 4
− 4
View file @ 31e28c22
......@@ -24,7 +24,7 @@ TEST(loop_closure_falko, TestLoopClosureFalkoAllFunctions)
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 0.3;
LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
LoopClosureFalkoAht<bsc, bscExtractor> loop_cl_falko(param);
// Test convert2LaserScanFALKO
std::shared_ptr<falkolib::LaserScan> scan_falko = loop_cl_falko.convert2LaserScanFALKO(scan, laser_params);
......@@ -81,7 +81,7 @@ TEST(loop_closure_falko, TestDescriptorsRotation)
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 0.3;
LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
LoopClosureFalkoAht<bsc, bscExtractor> loop_cl_falko(param);
// Test extractScene
auto new_scene = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_1, laser_params));
......@@ -194,7 +194,7 @@ TEST(loop_closure_falko, TestMatch)
ParameterLoopClosureFalko param;
param.matcher_distance_th_ = 100;
param.matcher_ddesc_th_ = 5;
LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko(param);
LoopClosureFalkoAht<bsc, bscExtractor> loop_cl_falko(param);
// Test extractScene
auto new_scene_1 = std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko.extractScene(scan_1, laser_params));
......@@ -227,7 +227,7 @@ TEST(loop_closure_falko, TestMatch)
laser_params.angle_step_ = 0.00701248;
LoopClosureFalko<bsc, bscExtractor, aht_matcher> loop_cl_falko_2(param);
LoopClosureFalkoAht<bsc, bscExtractor> loop_cl_falko_2(param);
auto new_scene_target =
std::static_pointer_cast<SceneFalko<bsc>>(loop_cl_falko_2.extractScene(scan_target, laser_params));
auto new_scene_reference =
......
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