Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
wolf
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
Container Registry
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
mobile_robotics
wolf_projects
wolf_lib
wolf
Commits
6c851df5
Commit
6c851df5
authored
2 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
uops
parent
bdd11183
No related branches found
No related tags found
1 merge request
!462
Resolve "Subscriber&processor for landmark external detections"
Pipeline
#13885
passed
2 years ago
Stage: deploy_plugins
Stage: deploy_ros
Changes
2
Pipelines
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/processor/processor_tracker_feature_landmark_external.cpp
+12
-2
12 additions, 2 deletions
...processor/processor_tracker_feature_landmark_external.cpp
test/gtest_processor_tracker_feature_landmark_external.cpp
+70
-41
70 additions, 41 deletions
test/gtest_processor_tracker_feature_landmark_external.cpp
with
82 additions
and
43 deletions
src/processor/processor_tracker_feature_landmark_external.cpp
+
12
−
2
View file @
6c851df5
...
@@ -175,9 +175,19 @@ bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
...
@@ -175,9 +175,19 @@ bool ProcessorTrackerFeatureLandmarkExternal::voteForKeyFrame() const
{
{
auto
matches_origin_last
=
track_matrix_
.
matches
(
origin_ptr_
,
last_ptr_
);
auto
matches_origin_last
=
track_matrix_
.
matches
(
origin_ptr_
,
last_ptr_
);
WOLF_INFO
(
"Nbr. of active feature tracks: "
,
matches_origin_last
.
size
()
);
// no tracks longer than filter_track_length_th
auto
n_tracks
=
0
;
for
(
auto
match
:
matches_origin_last
)
{
if
(
track_matrix_
.
track
(
match
.
first
).
size
()
>=
params_tfle_
->
filter_track_length_th
)
n_tracks
++
;
}
if
(
n_tracks
==
0
)
return
false
;
WOLF_INFO
(
"Nbr. of active feature tracks: "
,
n_tracks
);
bool
vote
=
matches_origin_last
.
size
()
<
params_tracker_feature_
->
min_features_for_keyframe
;
bool
vote
=
n_tracks
<
params_tracker_feature_
->
min_features_for_keyframe
;
if
(
origin_ptr_
)
if
(
origin_ptr_
)
{
{
...
...
This diff is collapsed.
Click to expand it.
test/gtest_processor_tracker_feature_landmark_external.cpp
+
70
−
41
View file @
6c851df5
...
@@ -65,7 +65,8 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
...
@@ -65,7 +65,8 @@ class ProcessorTrackerFeatureLandmarkExternalTest : public testing::Test
double
_dist_th
,
double
_dist_th
,
unsigned
int
_track_length_th
,
unsigned
int
_track_length_th
,
double
_time_span
);
double
_time_span
);
CaptureLandmarksExternalPtr
randomStep
();
void
randomStep
();
CaptureLandmarksExternalPtr
computeCaptureLandmarks
()
const
;
void
addRandomDetection
(
CaptureLandmarksExternalPtr
_cap
,
int
_id
,
double
_quality
)
const
;
void
addRandomDetection
(
CaptureLandmarksExternalPtr
_cap
,
int
_id
,
double
_quality
)
const
;
};
};
...
@@ -176,7 +177,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
...
@@ -176,7 +177,7 @@ void ProcessorTrackerFeatureLandmarkExternalTest::initProblem(int _dim,
o_sensor
=
sensor
->
getO
()
->
getState
();
o_sensor
=
sensor
->
getO
()
->
getState
();
}
}
CaptureLandmarksExternalPtr
ProcessorTrackerFeatureLandmarkExternalTest
::
randomStep
()
void
ProcessorTrackerFeatureLandmarkExternalTest
::
randomStep
()
{
{
// compute delta
// compute delta
VectorXd
delta
;
VectorXd
delta
;
...
@@ -198,7 +199,10 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS
...
@@ -198,7 +199,10 @@ CaptureLandmarksExternalPtr ProcessorTrackerFeatureLandmarkExternalTest::randomS
auto
state
=
processor_motion
->
getState
(
"PO"
);
auto
state
=
processor_motion
->
getState
(
"PO"
);
p_robot
=
state
.
vector
(
"P"
);
p_robot
=
state
.
vector
(
"P"
);
o_robot
=
state
.
vector
(
"O"
);
o_robot
=
state
.
vector
(
"O"
);
}
CaptureLandmarksExternalPtr
ProcessorTrackerFeatureLandmarkExternalTest
::
computeCaptureLandmarks
()
const
{
// Detections
// Detections
auto
cap
=
std
::
make_shared
<
CaptureLandmarksExternal
>
(
t
,
sensor
);
auto
cap
=
std
::
make_shared
<
CaptureLandmarksExternal
>
(
t
,
sensor
);
VectorXd
p_lmk
,
o_lmk
;
VectorXd
p_lmk
,
o_lmk
;
...
@@ -281,68 +285,93 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
...
@@ -281,68 +285,93 @@ TEST_F(ProcessorTrackerFeatureLandmarkExternalTest, check_p_2d)
double
dist_th
=
1e6
;
double
dist_th
=
1e6
;
int
track_length
=
5
;
int
track_length
=
5
;
double
time_span
=
3
*
dt
;
double
time_span
=
3
*
dt
;
bool
remove_landmarks
=
true
;
initProblem
(
dim
,
orientation
,
quality_th
,
dist_th
,
track_length
,
time_span
);
initProblem
(
dim
,
orientation
,
quality_th
,
dist_th
,
track_length
,
time_span
);
if
(
remove_landmarks
)
for
(
auto
lmk
:
landmarks
)
problem
->
getMap
()
->
removeLanlmk
->
remove
();
ASSERT_TRUE
(
problem
->
check
());
ASSERT_TRUE
(
problem
->
check
());
for
(
auto
i
=
0
;
i
<
10
;
i
++
)
for
(
auto
i
=
0
;
i
<
10
;
i
++
)
{
{
t
+=
dt
;
WOLF_INFO
(
"
\n
================= STEP "
,
i
,
" t = "
,
t
,
" ================="
);
WOLF_INFO
(
"STEP "
,
i
,
" t = "
,
t
,
" ================="
);
auto
cap
=
randomStep
();
// detection of landmarks
auto
cap
=
computeCaptureLandmarks
();
ASSERT_TRUE
(
problem
->
check
());
ASSERT_TRUE
(
problem
->
check
());
// process detections
cap
->
process
();
cap
->
process
();
ASSERT_TRUE
(
problem
->
check
());
ASSERT_TRUE
(
problem
->
check
());
//problem->print(4,1,1,1);
problem
->
print
(
4
,
1
,
1
,
1
);
// CHECKS
// Check vote for keyframe
if
(
t
.
getSeconds
()
>=
time_span
)
{
ASSERT_TRUE
(
problem
->
getTrajectory
()
->
size
()
>
1
);
}
FactorBasePtrList
fac_list
;
FactorBasePtrList
fac_list
;
problem
->
getTrajectory
()
->
getFactorList
(
fac_list
);
problem
->
getTrajectory
()
->
getFactorList
(
fac_list
);
if
(
i
>=
track_length
-
1
)
bool
should_emplace_KF
=
t
.
get
()
>=
time_span
and
i
>=
track_length
-
1
;
{
if
(
should_emplace_KF
)
// Check track_length
{
// voted for keyframe
ASSERT_TRUE
(
problem
->
getTrajectory
()
->
size
()
>
1
);
// emplaced factors
ASSERT_FALSE
(
fac_list
.
empty
());
ASSERT_FALSE
(
fac_list
.
empty
());
}
else
// factors' type
{
if
(
should_emplace_KF
)
// CHECK track_length
for
(
auto
fac
:
fac_list
)
ASSERT_TRUE
(
fac_list
.
empty
());
// Check factors
for
(
auto
fac
:
fac_list
)
{
// Correct type
if
(
fac
->
getProcessor
()
==
processor
)
{
{
if
(
dim
==
2
and
orientation
)
if
(
fac
->
getProcessor
()
==
processor
)
{
ASSERT_EQ
(
fac
->
getType
(),
"FactorRelativePose2dWithExtrinsiscs"
);
}
if
(
dim
==
3
and
not
orientation
)
{
{
ASSERT_EQ
(
fac
->
getType
(),
"FactorRelativePose3dWithExtrinsiscs"
);
ASSERT_EQ
(
fac
->
getType
(),
std
::
string
(
"FactorRelative"
)
+
}
(
orientation
?
"Pose"
:
"Position"
)
+
if
(
dim
==
2
and
orientation
)
(
dim
==
2
?
"2d"
:
"3d"
)
+
{
"WithExtrinsiscs"
);
ASSERT_EQ
(
fac
->
getType
(),
"FactorRelativePosition2dWithExtrinsiscs"
);
}
if
(
dim
==
3
and
not
orientation
)
{
ASSERT_EQ
(
fac
->
getType
(),
"FactorRelativePosition3dWithExtrinsiscs"
);
}
}
}
}
// landmarks
auto
landmarks_map
=
problem
->
getMap
()
->
getLandmarkList
();
ASSERT_EQ
(
landmarks_map
.
size
(),
landmarks
.
size
());
for
(
auto
lmk_map
:
landmarks_map
)
{
ASSERT_TRUE
(
lmk_map
->
id
()
<
landmarks
.
size
());
auto
lmk_gt
=
landmarks
.
at
(
lmk_map
->
id
());
ASSERT_EQ
(
lmk_map
->
id
(),
lmk_gt
->
id
());
if
(
dim
==
2
and
orientation
)
{
ASSERT_POSE2d_APPROX
(
lmk_map
->
getState
().
vector
(
"PO"
),
lmk_gt
->
getState
().
vector
(
"PO"
),
Constants
::
EPS
);
}
else
if
(
dim
==
2
and
not
orientation
)
{
ASSERT_MATRIX_APPROX
(
lmk_map
->
getState
().
vector
(
"P"
),
lmk_gt
->
getState
().
vector
(
"P"
),
Constants
::
EPS
);
}
else
if
(
dim
==
3
and
orientation
)
{
ASSERT_POSE3d_APPROX
(
lmk_map
->
getState
().
vector
(
"PO"
),
lmk_gt
->
getState
().
vector
(
"PO"
),
Constants
::
EPS
);
}
else
if
(
dim
==
3
and
not
orientation
)
{
ASSERT_MATRIX_APPROX
(
lmk_map
->
getState
().
vector
(
"P"
),
lmk_gt
->
getState
().
vector
(
"P"
),
Constants
::
EPS
);
}
}
}
}
}
else
{
// didn't vote for keyframe
ASSERT_FALSE
(
problem
->
getTrajectory
()
->
size
()
>
1
);
// no factors emplaced
ASSERT_TRUE
(
fac_list
.
empty
());
// landmarks
ASSERT_EQ
(
problem
->
getMap
()
->
getLandmarkList
().
empty
(),
remove_landmarks
);
}
// step with random movement
t
+=
dt
;
randomStep
();
}
}
}
}
...
...
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