Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
laser_scan_utils
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Model registry
Operate
Environments
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor analytics
CI/CD analytics
Repository analytics
Model experiments
Help
Help
Support
GitLab documentation
Compare GitLab plans
Community forum
Contribute to GitLab
Provide feedback
Keyboard shortcuts
?
Snippets
Groups
Projects
Show more breadcrumbs
labrobotica
algorithms
laser_scan_utils
Commits
6f7311ce
Commit
6f7311ce
authored
4 years ago
by
Sergi Pujol Badell
Browse files
Options
Downloads
Patches
Plain Diff
renaming use_descriptors
parent
20a993d6
No related branches found
No related tags found
1 merge request
!4
Resolve "Implementation of Falko lib"
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/loop_closure_falko.h
+141
-138
141 additions, 138 deletions
src/loop_closure_falko.h
test/gtest_loop_closure_falko.cpp
+51
-58
51 additions, 58 deletions
test/gtest_loop_closure_falko.cpp
with
192 additions
and
196 deletions
src/loop_closure_falko.h
+
141
−
138
View file @
6f7311ce
...
@@ -42,152 +42,155 @@ namespace laserscanutils {
...
@@ -42,152 +42,155 @@ namespace laserscanutils {
typedef
falkolib
::
BSCExtractor
<
falkolib
::
FALKO
>
bscExtractor
;
typedef
falkolib
::
BSCExtractor
<
falkolib
::
FALKO
>
bscExtractor
;
typedef
falkolib
::
CGHExtractor
<
falkolib
::
FALKO
>
cghExtractor
;
typedef
falkolib
::
CGHExtractor
<
falkolib
::
FALKO
>
cghExtractor
;
template
<
typename
T
,
typename
D
>
using
nn_matcher
=
falkolib
::
NNMatcher
<
T
,
D
>
;
template
<
typename
T
,
typename
D
>
using
nn_matcher
=
falkolib
::
NNMatcher
<
T
,
D
>
;
template
<
typename
T
,
typename
D
>
template
<
typename
T
,
typename
D
>
using
aht_matcher
=
falkolib
::
AHTMatcher
<
T
,
D
>
;
using
aht_matcher
=
falkolib
::
AHTMatcher
<
T
,
D
>
;
struct
ParameterLoopClosureFalko
struct
ParameterLoopClosureFalko
{
{
// Keypoints extractor Default
// Keypoints extractor Default
double
min_extraction_range_
=
0.1
;
double
min_extraction_range_
=
0.1
;
double
max_extraction_range_
=
25
;
double
max_extraction_range_
=
25
;
bool
enable_subbeam_
=
true
;
bool
enable_subbeam_
=
true
;
double
nms_radius_
=
0.1
;
double
nms_radius_
=
0.1
;
double
neigh_b_
=
0.01
;
double
neigh_b_
=
0.01
;
double
b_ratio_
=
4
;
double
b_ratio_
=
4
;
int
grid_sectors_
=
16
;
int
grid_sectors_
=
16
;
// Descriptors parameters Default
// Descriptors parameters Default
int
circularSectorNumber_
=
16
;
int
circularSectorNumber_
=
16
;
int
radialRingNumber_
=
8
;
int
radialRingNumber_
=
8
;
// matcher threshold Default
// matcher threshold Default
double
matcher_distance_th_
=
0.2
;
double
matcher_distance_th_
=
0.2
;
int
keypoints_number_th_
=
5
;
int
keypoints_number_th_
=
5
;
int
match_type_
=
1
;
// match_type=1-> uses keypoints and descriptors for
bool
use_descriptors_
=
1
;
// match_type=1-> uses keypoints and descriptors
// matching. match_type=2-> uses only keypoints for
// match_type=0-> uses only keypoints
// matching
double
matcher_ddesc_th_
=
0.2
;
// matching
double
matcher_ddesc_th_
=
0.2
;
// aht matcher
double
xRes_
=
0.1
;
// aht matcher
double
yRes_
=
0.1
;
double
xRes_
=
0.1
;
double
thetaRes_
=
0.04
;
double
yRes_
=
0.1
;
double
xAbsMax_
=
5
;
double
thetaRes_
=
0.04
;
double
yAbsMax_
=
5
;
double
xAbsMax_
=
5
;
double
thetaAbsMax_
=
1.57
;
double
yAbsMax_
=
5
;
double
thetaAbsMax_
=
1.57
;
};
};
/** \brief A base class for loop closure using falko library
/** \brief A base class for loop closure using falko library
**/
**/
template
<
typename
D
,
typename
Extr
,
template
<
typename
,
typename
>
typename
M
>
template
<
typename
D
,
typename
Extr
,
template
<
typename
,
typename
>
typename
M
>
class
LoopClosureFalko
:
public
LoopClosureBase2d
,
class
LoopClosureFalko
:
public
LoopClosureBase2d
,
public
falkolib
::
FALKOExtractor
public
falkolib
::
FALKOExtractor
{
{
private:
private:
public:
public:
typedef
std
::
shared_ptr
<
SceneFalko
<
D
>>
sceneFalkoBSCPtr
;
typedef
std
::
shared_ptr
<
SceneFalko
<
D
>>
sceneFalkoBSCPtr
;
typedef
std
::
shared_ptr
<
falkolib
::
LaserScan
>
laserScanPtr
;
typedef
std
::
shared_ptr
<
falkolib
::
LaserScan
>
laserScanPtr
;
Extr
extractor_
;
Extr
extractor_
;
M
<
falkolib
::
FALKO
,
D
>
matcher_
;
M
<
falkolib
::
FALKO
,
D
>
matcher_
;
// M<D, D> matcher_desc_;
// M<D, D> matcher_desc_;
/** \brief Constructor for nn_matcher
/** \brief Constructor for nn_matcher
**/
**/
LoopClosureFalko
(
ParameterLoopClosureFalko
_param
)
LoopClosureFalko
(
ParameterLoopClosureFalko
_param
)
:
LoopClosureBase2d
(),
falkolib
::
FALKOExtractor
(),
:
LoopClosureBase2d
()
extractor_
(
_param
.
circularSectorNumber_
,
_param
.
radialRingNumber_
),
,
falkolib
::
FALKOExtractor
()
matcher_
()
{
,
extractor_
(
_param
.
circularSectorNumber_
,
_param
.
radialRingNumber_
)
// FALKO Extractor Parameters
,
matcher_
()
setMinExtractionRange
(
_param
.
min_extraction_range_
);
{
setMaxExtractionRange
(
_param
.
max_extraction_range_
);
// FALKO Extractor Parameters
enableSubbeam
(
_param
.
enable_subbeam_
);
setMinExtractionRange
(
_param
.
min_extraction_range_
);
setNMSRadius
(
_param
.
nms_radius_
);
setMaxExtractionRange
(
_param
.
max_extraction_range_
);
setNeighB
(
_param
.
neigh_b_
);
enableSubbeam
(
_param
.
enable_subbeam_
);
setBRatio
(
_param
.
b_ratio_
);
setNMSRadius
(
_param
.
nms_radius_
);
setGridSectors
(
_param
.
grid_sectors_
);
setNeighB
(
_param
.
neigh_b_
);
setBRatio
(
_param
.
b_ratio_
);
// Matcher Extractor Parameters
setGridSectors
(
_param
.
grid_sectors_
);
matcher_
.
setDistanceThreshold
(
_param
.
matcher_distance_th_
);
matcher_
.
setDescriptorThreshold
(
_param
.
matcher_ddesc_th_
);
// Matcher Extractor Parameters
keypoints_number_th_
=
_param
.
keypoints_number_th_
;
matcher_
.
setDistanceThreshold
(
_param
.
matcher_distance_th_
);
match_type_
=
_param
.
match_type_
;
matcher_
.
setDescriptorThreshold
(
_param
.
matcher_ddesc_th_
);
};
keypoints_number_th_
=
_param
.
keypoints_number_th_
;
use_descriptors_
=
_param
.
use_descriptors_
;
/** \brief Destructor
};
**/
~
LoopClosureFalko
()
{}
/** \brief Destructor
**/
/** \brief Create and update the scene struct with keypoints and descriptors
~
LoopClosureFalko
()
{}
**/
sceneBasePtr
extractScene
(
const
LaserScan
&
_scan
,
/** \brief Create and update the scene struct with keypoints and descriptors
const
LaserScanParams
&
_scan_params
)
override
{
**/
auto
new_scene
=
std
::
make_shared
<
SceneFalko
<
D
>>
();
sceneBasePtr
extractScene
(
const
LaserScan
&
_scan
,
const
LaserScanParams
&
_scan_params
)
override
auto
scan_falko
=
convert2LaserScanFALKO
(
_scan
,
_scan_params
);
{
// Extract keypoints
auto
new_scene
=
std
::
make_shared
<
SceneFalko
<
D
>>
();
extract
(
*
scan_falko
,
(
new_scene
->
keypoints_list_
));
auto
scan_falko
=
convert2LaserScanFALKO
(
_scan
,
_scan_params
);
// Compute descriptors
// Extract keypoints
extractor_
.
compute
(
*
scan_falko
,
new_scene
->
keypoints_list_
,
extract
(
*
scan_falko
,
(
new_scene
->
keypoints_list_
));
new_scene
->
descriptors_list_
);
// Compute descriptors
return
new_scene
;
extractor_
.
compute
(
*
scan_falko
,
new_scene
->
keypoints_list_
,
new_scene
->
descriptors_list_
);
}
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
**/
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
(
match_type_
==
1
)
{
matching_number
=
matcher_
.
match
(
scene_1_falko
->
keypoints_list_
,
scene_2_falko
->
keypoints_list_
,
asso_nn
);
}
else
if
(
match_type_
==
2
)
{
matching_number
=
matcher_
.
match
(
scene_1_falko
->
keypoints_list_
,
scene_1_falko
->
descriptors_list_
,
scene_2_falko
->
keypoints_list_
,
scene_2_falko
->
descriptors_list_
,
asso_nn
);
}
}
auto
new_match
=
std
::
make_shared
<
MatchLoopClosureScene
>
();
new_match
->
keypoints_number_match
=
matching_number
;
/** \brief Convert scans from laserscanutils::LaserScan to
if
(
matching_number
>
keypoints_number_th_
)
{
*falkolib::LaserScan object
new_match
->
match
=
computeTransform
(
scene_1_falko
->
keypoints_list_
,
**/
scene_2_falko
->
keypoints_list_
,
laserScanPtr
convert2LaserScanFALKO
(
const
LaserScan
&
_scan
,
const
LaserScanParams
&
_scan_params
)
asso_nn
,
new_match
->
transform
);
{
}
else
{
auto
scan_falko
=
std
::
make_shared
<
falkolib
::
LaserScan
>
(
_scan_params
.
angle_min_
,
_scan_params
.
angle_max_
,
new_match
->
match
=
false
;
_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
**/
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_
==
1
)
{
matching_number
=
matcher_
.
match
(
scene_1_falko
->
keypoints_list_
,
scene_2_falko
->
keypoints_list_
,
asso_nn
);
}
else
if
(
use_descriptors_
==
0
)
{
matching_number
=
matcher_
.
match
(
scene_1_falko
->
keypoints_list_
,
scene_1_falko
->
descriptors_list_
,
scene_2_falko
->
keypoints_list_
,
scene_2_falko
->
descriptors_list_
,
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
->
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
());
//}
return
new_match
;
}
}
new_match
->
scene_1
=
_scene_1
;
new_match
->
scene_2
=
_scene_2
;
int
keypoints_number_th_
;
bool
use_descriptors_
;
new_match
->
score
=
(
double
)
matching_number
/
(
double
)
std
::
min
(
scene_1_falko
->
keypoints_list_
.
size
(),
scene_2_falko
->
keypoints_list_
.
size
());
//}
return
new_match
;
}
int
keypoints_number_th_
;
int
match_type_
;
};
};
}
/* namespace laserscanutils */
}
/* namespace laserscanutils */
...
...
This diff is collapsed.
Click to expand it.
test/gtest_loop_closure_falko.cpp
+
51
−
58
View file @
6f7311ce
...
@@ -5,64 +5,57 @@
...
@@ -5,64 +5,57 @@
using
namespace
laserscanutils
;
using
namespace
laserscanutils
;
TEST
(
loop_closure_falko
,
TestLoopClosureFalkoAllFunctions
)
{
TEST
(
loop_closure_falko
,
TestLoopClosureFalkoAllFunctions
)
// Initialization
{
int
scan_size
=
1440
;
// Initialization
LaserScan
scan
,
scan2
;
int
scan_size
=
1440
;
LaserScanParams
laser_params
;
LaserScan
scan
,
scan2
;
laser_params
.
angle_min_
=
0
;
LaserScanParams
laser_params
;
laser_params
.
angle_max_
=
2.0
*
M_PI
;
laser_params
.
angle_min_
=
0
;
for
(
int
i
=
0
;
i
<
scan_size
;
i
++
)
{
laser_params
.
angle_max_
=
2.0
*
M_PI
;
scan
.
ranges_raw_
.
push_back
(
testRanges1
[
i
]);
for
(
int
i
=
0
;
i
<
scan_size
;
i
++
)
scan2
.
ranges_raw_
.
push_back
(
testRanges2
[
i
]);
{
}
scan
.
ranges_raw_
.
push_back
(
testRanges1
[
i
]);
scan2
.
ranges_raw_
.
push_back
(
testRanges2
[
i
]);
ParameterLoopClosureFalko
param
;
}
LoopClosureFalko
<
bsc
,
bscExtractor
,
nn_matcher
>
loop_cl_falko
(
param
);
ParameterLoopClosureFalko
param
;
// Test convert2LaserScanFALKO
LoopClosureFalko
<
bsc
,
bscExtractor
,
nn_matcher
>
loop_cl_falko
(
param
);
std
::
shared_ptr
<
falkolib
::
LaserScan
>
scan_falko
=
loop_cl_falko
.
convert2LaserScanFALKO
(
scan
,
laser_params
);
// Test convert2LaserScanFALKO
int
firstPoint
=
scan_falko
->
ranges
[
0
];
std
::
shared_ptr
<
falkolib
::
LaserScan
>
scan_falko
=
loop_cl_falko
.
convert2LaserScanFALKO
(
scan
,
laser_params
);
int
firstPoint
=
scan_falko
->
ranges
[
0
];
ASSERT_EQ
(
firstPoint
,
250
);
ASSERT_EQ
(
firstPoint
,
250
);
// Test extractScene
auto
new_scene
=
std
::
static_pointer_cast
<
SceneFalko
<
bsc
>>
(
// Test extractScene
loop_cl_falko
.
extractScene
(
scan
,
laser_params
));
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
>>
(
auto
new_scene2
=
std
::
static_pointer_cast
<
SceneFalko
<
bsc
>>
(
loop_cl_falko
.
extractScene
(
scan2
,
laser_params
));
loop_cl_falko
.
extractScene
(
scan2
,
laser_params
));
int
detectedKeypoints
=
new_scene
->
keypoints_list_
.
size
();
int
detectedKeypoints
=
new_scene
->
keypoints_list_
.
size
();
int
detectedDescriptors
=
new_scene
->
descriptors_list_
.
size
();
int
detectedDescriptors
=
new_scene
->
descriptors_list_
.
size
();
ASSERT_EQ
(
detectedKeypoints
,
18
);
ASSERT_EQ
(
detectedKeypoints
,
18
);
ASSERT_EQ
(
detectedDescriptors
,
18
);
ASSERT_EQ
(
detectedDescriptors
,
18
);
// Test matcheScene
// Test matcheScene
auto
new_match
=
loop_cl_falko
.
matchScene
(
new_scene
,
new_scene
);
auto
new_match
=
loop_cl_falko
.
matchScene
(
new_scene
,
new_scene
);
ASSERT_EQ
(
new_match
->
keypoints_number_match
,
18
);
ASSERT_EQ
(
new_match
->
keypoints_number_match
,
18
);
std
::
list
<
std
::
shared_ptr
<
SceneBase
>>
ref_scenes
;
// TesT findLoopClosure
ref_scenes
.
emplace_back
(
new_scene
);
//auto ref_scenes = std::make_shared<SceneFalkoList<bsc>>();
ref_scenes
.
emplace_back
(
new_scene2
);
//ref_scenes->scenes_.emplace_back(new_scene);
//ref_scenes->scenes_.emplace_back(new_scene2);
auto
matchings
=
loop_cl_falko
.
findLoopClosure
(
ref_scenes
,
new_scene
);
std
::
list
<
std
::
shared_ptr
<
SceneBase
>>
ref_scenes
;
ASSERT_EQ
(
matchings
.
size
(),
2
);
ref_scenes
.
emplace_back
(
new_scene
);
auto
best_match
=
matchings
.
rbegin
()
->
second
;
ref_scenes
.
emplace_back
(
new_scene2
);
ASSERT_EQ
(
best_match
->
match
,
true
);
ASSERT_EQ
(
best_match
->
scene_1
,
new_scene
);
auto
matchings
=
loop_cl_falko
.
findLoopClosure
(
ref_scenes
,
new_scene
);
ASSERT_EQ
(
best_match
->
scene_2
,
new_scene
);
ASSERT_EQ
(
best_match
->
score
,
1
);
ASSERT_EQ
(
matchings
.
size
(),
2
);
auto
best_match
=
matchings
.
rbegin
()
->
second
;
ASSERT_EQ
(
best_match
->
match
,
true
);
ASSERT_EQ
(
best_match
->
scene_1
,
new_scene
);
ASSERT_EQ
(
best_match
->
scene_2
,
new_scene
);
ASSERT_EQ
(
best_match
->
score
,
1
);
// PRINTF("All good at TestTest::DummyTestExample !\n");
}
}
int
main
(
int
argc
,
char
**
argv
)
{
int
main
(
int
argc
,
char
**
argv
)
testing
::
InitGoogleTest
(
&
argc
,
argv
);
{
return
RUN_ALL_TESTS
();
testing
::
InitGoogleTest
(
&
argc
,
argv
);
return
RUN_ALL_TESTS
();
}
}
This diff is collapsed.
Click to expand it.
Preview
0%
Loading
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Save comment
Cancel
Please
register
or
sign in
to comment