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
5c1195bc
Commit
5c1195bc
authored
4 years ago
by
Sergi Pujol Badell
Browse files
Options
Downloads
Patches
Plain Diff
computed max_dist between keypoints of an scene
parent
c5e90bda
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
+105
-6
105 additions, 6 deletions
src/loop_closure_falko.h
src/scene_base.h
+6
-0
6 additions, 0 deletions
src/scene_base.h
with
111 additions
and
6 deletions
src/loop_closure_falko.h
+
105
−
6
View file @
5c1195bc
...
@@ -74,17 +74,17 @@ struct ParameterLoopClosureFalko
...
@@ -74,17 +74,17 @@ struct ParameterLoopClosureFalko
};
};
/** \brief A class for loop closure using falko library
/** \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
* 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 extracts scenes from a laserscanutils::LaserScan. The scenes contain keypoints and descriptors
*
*
* It matches a target scene against a list of reference scenes.
* It matches a target scene against a list of reference scenes.
*
*
* The reference scenes are found from a search of the previous captures
* 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.
* Diferent types of descriptors can be used, and are specified as template parameters.
*
*
* \tparam D Descriptor type. <bsc> or <cgh>
* \tparam D Descriptor type. <bsc> or <cgh>
* \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor>
* \tparam Extr descriptor extractor type <bscExtractor> or <cghExtractor>
* \tparam M Matcher type <nn_matcher> or <aht_matcher>
* \tparam M Matcher type <nn_matcher> or <aht_matcher>
...
@@ -141,6 +141,87 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
...
@@ -141,6 +141,87 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
extract
(
*
scan_falko
,
(
new_scene
->
keypoints_list_
));
extract
(
*
scan_falko
,
(
new_scene
->
keypoints_list_
));
// Compute descriptors
// Compute descriptors
extractor_
.
compute
(
*
scan_falko
,
new_scene
->
keypoints_list_
,
new_scene
->
descriptors_list_
);
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
;
return
new_scene
;
}
}
...
@@ -205,6 +286,24 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
...
@@ -205,6 +286,24 @@ class LoopClosureFalko : public LoopClosureBase2d, public falkolib::FALKOExtract
int
keypoints_number_th_
;
int
keypoints_number_th_
;
bool
use_descriptors_
;
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 */
}
/* namespace laserscanutils */
...
...
This diff is collapsed.
Click to expand it.
src/scene_base.h
+
6
−
0
View file @
5c1195bc
...
@@ -16,6 +16,12 @@ namespace laserscanutils {
...
@@ -16,6 +16,12 @@ namespace laserscanutils {
struct
SceneBase
struct
SceneBase
{
{
int
id
;
int
id
;
double
area_
;
double
perimeter_
;
double
max_distance_
;
double
mean_distance_
;
Eigen
::
Vector2d
mid_point_
;
};
};
typedef
std
::
shared_ptr
<
SceneBase
>
sceneBasePtr
;
typedef
std
::
shared_ptr
<
SceneBase
>
sceneBasePtr
;
...
...
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