Skip to content
GitLab
Menu
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in
Toggle navigation
Menu
Open sidebar
mobile_robotics
wolf_projects
wolf_lib
plugins
vision
Commits
73673fda
Commit
73673fda
authored
Apr 23, 2022
by
Joan Solà Ortega
Browse files
Reduce debug printed info. leave only the TRACKS bar
parent
66bf4dd5
Changes
1
Hide whitespace changes
Inline
Side-by-side
src/processor/processor_visual_odometry.cpp
View file @
73673fda
...
...
@@ -121,7 +121,7 @@ void ProcessorVisualOdometry::preProcess()
}
}
}
WOLF_
INFO
(
"Initially detected "
,
capture_image_incoming_
->
getKeyPoints
().
size
(),
" keypoints in incoming"
);
WOLF_
DEBUG
(
"Initially detected "
,
capture_image_incoming_
->
getKeyPoints
().
size
(),
" keypoints in incoming"
);
// Initialize the tracks data structure with a "dummy track" where the keypoint is pointing to itself
TracksMap
tracks_init
;
...
...
@@ -137,10 +137,10 @@ void ProcessorVisualOdometry::preProcess()
{
s
+=
"#"
;
}
WOLF_INFO
(
"TRACKS: "
,
s
);
WOLF_INFO
(
"TRACKS: "
,
capture_image_incoming_
->
getKeyPoints
().
size
(),
" "
,
s
);
auto
dt_preprocess
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
system_clock
::
now
()
-
t1
).
count
();
WOLF_
INFO
(
"dt_preprocess (ms): "
,
dt_preprocess
);
auto
__attribute__
((
unused
))
dt_preprocess
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
system_clock
::
now
()
-
t1
).
count
();
WOLF_
DEBUG
(
"dt_preprocess (ms): "
,
dt_preprocess
);
return
;
}
...
...
@@ -165,13 +165,13 @@ void ProcessorVisualOdometry::preProcess()
WOLF_
INFO
(
"Tracking "
,
mwkps_last
.
size
(),
" keypoints in last"
);
WOLF_
DEBUG
(
"Tracking "
,
mwkps_last
.
size
(),
" keypoints in last"
);
// TracksMap between last and incoming
// Input: ID of Wkp in last. Output: ID of the tracked Wkp in incoming.
TracksMap
tracks_last_incoming
=
kltTrack
(
img_last
,
img_incoming
,
mwkps_last
,
mwkps_incoming
);
// WOLF_
INFO
( "Tracked " , mwkps_incoming.size(), " keypoints to incoming" );
// WOLF_
DEBUG
( "Tracked " , mwkps_incoming.size(), " keypoints to incoming" );
// TracksMap between origin and last
// Input: ID of Wkp in origin. Output: ID of the tracked Wkp in last.
...
...
@@ -181,7 +181,7 @@ void ProcessorVisualOdometry::preProcess()
// Input: ID of Wkp in origin. Output: ID of the tracked Wkp in incoming.
TracksMap
tracks_origin_incoming
=
mergeTracks
(
tracks_origin_last
,
tracks_last_incoming
);
// WOLF_
INFO
( "Merged " , tracks_last_incoming.size(), " tracks..." );
// WOLF_
DEBUG
( "Merged " , tracks_last_incoming.size(), " tracks..." );
// Outliers rejection with essential matrix
cv
::
Mat
E
;
...
...
@@ -200,7 +200,7 @@ void ProcessorVisualOdometry::preProcess()
}
}
}
WOLF_
INFO
(
"Tracked "
,
mwkps_incoming_filtered
.
size
(),
" inliers in incoming"
);
WOLF_
DEBUG
(
"Tracked "
,
mwkps_incoming_filtered
.
size
(),
" inliers in incoming"
);
////////////////////////////////
...
...
@@ -209,12 +209,12 @@ void ProcessorVisualOdometry::preProcess()
////////////////////////////////
size_t
n_tracks_origin
=
tracks_origin_incoming
.
size
();
// WOLF_
INFO
("# of tracks: ", n_tracks_origin, "; min # of tracks: ", params_visual_odometry_->min_features_for_keyframe);
// WOLF_
DEBUG
("# of tracks: ", n_tracks_origin, "; min # of tracks: ", params_visual_odometry_->min_features_for_keyframe);
if
(
n_tracks_origin
<
params_visual_odometry_
->
min_features_for_keyframe
)
{
// WOLF_
INFO
( "Too Few Tracks. Detecting more keypoints in last" );
// WOLF_
DEBUG
( "Too Few Tracks. Detecting more keypoints in last" );
// Erase all keypoints previously added to the cell grid
cell_grid_
.
renew
();
...
...
@@ -265,7 +265,7 @@ void ProcessorVisualOdometry::preProcess()
}
}
// WOLF_
INFO
("Detected ", kps_last_new.size(), " new raw keypoints");
// WOLF_
DEBUG
("Detected ", kps_last_new.size(), " new raw keypoints");
// Create a map of wolf KeyPoints to track only the new ones
KeyPointsMap
mwkps_last_new
,
mwkps_incoming_new
;
...
...
@@ -273,13 +273,13 @@ void ProcessorVisualOdometry::preProcess()
WKeyPoint
wkp
(
cvkp
);
mwkps_last_new
[
wkp
.
getId
()]
=
wkp
;
}
WOLF_
INFO
(
"Detected "
,
mwkps_last_new
.
size
(),
" new keypoints in last"
);
WOLF_
DEBUG
(
"Detected "
,
mwkps_last_new
.
size
(),
" new keypoints in last"
);
TracksMap
tracks_last_incoming_new
=
kltTrack
(
img_last
,
img_incoming
,
mwkps_last_new
,
mwkps_incoming_new
);
WOLF_
INFO
(
"Tracked "
,
mwkps_incoming_new
.
size
(),
" inliers in incoming"
);
WOLF_
DEBUG
(
"Tracked "
,
mwkps_incoming_new
.
size
(),
" inliers in incoming"
);
// WOLF_
INFO
("Tracked ", mwkps_incoming_new.size(), " new keypoints to incoming");
// WOLF_
DEBUG
("Tracked ", mwkps_incoming_new.size(), " new keypoints to incoming");
// Concatenation of old tracks and new tracks
for
(
auto
&
track
:
tracks_last_incoming_new
){
...
...
@@ -292,7 +292,7 @@ void ProcessorVisualOdometry::preProcess()
// tracks that are not geometrically consistent are removed from tracks_last_incoming_new
filterWithEssential
(
mwkps_last_filtered
,
mwkps_incoming_filtered
,
tracks_last_incoming_filtered
,
E
);
WOLF_
INFO
(
"New total : "
,
n_tracks_origin
,
" + "
,
mwkps_incoming_new
.
size
(),
" = "
,
tracks_last_incoming_filtered
.
size
(),
" tracks"
);
WOLF_
DEBUG
(
"New total : "
,
n_tracks_origin
,
" + "
,
mwkps_incoming_new
.
size
(),
" = "
,
tracks_last_incoming_filtered
.
size
(),
" tracks"
);
// add a flag so that voteForKeyFrame use it to vote for a KeyFrame
capture_image_incoming_
->
setLastWasRepopulated
(
true
);
...
...
@@ -303,7 +303,7 @@ void ProcessorVisualOdometry::preProcess()
}
else
{
WOLF_
INFO
(
"
\n\n
"
);
WOLF_
DEBUG
(
"
\n\n
"
);
}
// Update captures
...
...
@@ -311,7 +311,7 @@ void ProcessorVisualOdometry::preProcess()
capture_image_incoming_
->
setTracksPrev
(
tracks_last_incoming_filtered
);
capture_image_incoming_
->
setTracksOrigin
(
tracks_origin_incoming
);
auto
dt_preprocess
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
system_clock
::
now
()
-
t1
).
count
();
auto
__attribute__
((
unused
))
dt_preprocess
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
system_clock
::
now
()
-
t1
).
count
();
// print a bar with the number of active tracks in incoming
std
::
string
s
;
...
...
@@ -319,9 +319,9 @@ void ProcessorVisualOdometry::preProcess()
{
s
+=
"#"
;
}
WOLF_INFO
(
"TRACKS: "
,
s
);
WOLF_INFO
(
"TRACKS: "
,
capture_image_incoming_
->
getKeyPoints
().
size
(),
" "
,
s
);
WOLF_
INFO
(
"dt_preprocess (ms): "
,
dt_preprocess
);
WOLF_
DEBUG
(
"dt_preprocess (ms): "
,
dt_preprocess
);
return
;
}
...
...
@@ -346,7 +346,7 @@ unsigned int ProcessorVisualOdometry::processKnown()
// otherwise we store the pair as a newly detected track (for processNew)
TracksMap
tracks_map_li
=
capture_image_incoming_
->
getTracksPrev
();
if
(
tracks_map_li
.
count
(
id_feat_last
)){
// WOLF_
INFO
("A corresponding track has been found for id_feat_last ", id_feat_last );
// WOLF_
DEBUG
("A corresponding track has been found for id_feat_last ", id_feat_last );
auto
kp_track_li
=
tracks_map_li
.
find
(
id_feat_last
);
// create a feature using the corresponding WKeyPoint contained in incoming (hence the "second")
auto
feat_inco
=
FeatureBase
::
emplace
<
FeaturePointImage
>
(
...
...
@@ -361,8 +361,8 @@ unsigned int ProcessorVisualOdometry::processKnown()
tracks_map_li_matched_
.
insert
(
kp_track_li_matched
);
}
}
auto
dt_processKnown
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
system_clock
::
now
()
-
t1
).
count
();
WOLF_
INFO
(
"dt_processKnown (ms): "
,
dt_processKnown
);
auto
__attribute__
((
unused
))
dt_processKnown
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
system_clock
::
now
()
-
t1
).
count
();
WOLF_
DEBUG
(
"dt_processKnown (ms): "
,
dt_processKnown
);
// return number of successful tracks until incoming
return
tracks_map_li_matched_
.
size
();
...
...
@@ -401,8 +401,8 @@ unsigned int ProcessorVisualOdometry::processNew(const int& _max_features)
}
}
auto
dt_processNew
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
system_clock
::
now
()
-
t1
).
count
();
WOLF_
INFO
(
"dt_processNew (ms): "
,
dt_processNew
);
auto
__attribute__
((
unused
))
dt_processNew
=
std
::
chrono
::
duration_cast
<
std
::
chrono
::
milliseconds
>
(
std
::
chrono
::
system_clock
::
now
()
-
t1
).
count
();
WOLF_
DEBUG
(
"dt_processNew (ms): "
,
dt_processNew
);
return
counter_new_tracks
;
}
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Attach a file
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment