Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
V
vision
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Deploy
Releases
Model registry
Monitor
Incidents
Service Desk
Analyze
Value stream analytics
Contributor 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
plugins
vision
Commits
ed1dcae3
Commit
ed1dcae3
authored
3 years ago
by
César DEBEUNNE
Browse files
Options
Downloads
Patches
Plain Diff
Edit PreProcess()
parent
29396bcc
No related branches found
No related tags found
2 merge requests
!36
After cmake and const refactor
,
!28
Resolve "Building a new visual odometry system"
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/vision/processor/processor_visual_odometry.h
+7
-3
7 additions, 3 deletions
include/vision/processor/processor_visual_odometry.h
src/processor/processor_visual_odometry.cpp
+67
-11
67 additions, 11 deletions
src/processor/processor_visual_odometry.cpp
with
74 additions
and
14 deletions
include/vision/processor/processor_visual_odometry.h
+
7
−
3
View file @
ed1dcae3
...
@@ -85,6 +85,7 @@ class ProcessorVisualOdometry : public ProcessorTracker
...
@@ -85,6 +85,7 @@ class ProcessorVisualOdometry : public ProcessorTracker
// A few casted smart pointers
// A few casted smart pointers
CaptureImagePtr
capture_image_last_
;
CaptureImagePtr
capture_image_last_
;
CaptureImagePtr
capture_image_incoming_
;
CaptureImagePtr
capture_image_incoming_
;
CaptureImagePtr
capture_image_origin_
;
SensorCameraPtr
sen_cam_
;
SensorCameraPtr
sen_cam_
;
private
:
private
:
...
@@ -107,7 +108,6 @@ class ProcessorVisualOdometry : public ProcessorTracker
...
@@ -107,7 +108,6 @@ class ProcessorVisualOdometry : public ProcessorTracker
int
search_height_
;
int
search_height_
;
int
pyramid_level_
;
int
pyramid_level_
;
// camera
// camera
cv
::
Mat
Kcv_
;
cv
::
Mat
Kcv_
;
...
@@ -167,9 +167,13 @@ class ProcessorVisualOdometry : public ProcessorTracker
...
@@ -167,9 +167,13 @@ class ProcessorVisualOdometry : public ProcessorTracker
*/
*/
void
resetDerived
()
override
;
void
resetDerived
()
override
;
/** \brief Implementation of pyramidal KLT
/** \brief Implementation of pyramidal KLT with openCV
*/
TracksMap
kltTrack
(
cv
::
Mat
img_prev
,
cv
::
Mat
img_curr
,
KeyPointsMap
&
mwkps_prev
,
KeyPointsMap
&
mwkps_curr
);
/** \brief Implementation of 5 point algorithm with openCV, remove outliers from the tracks map
*/
*/
TracksMap
klt_track
(
cv
::
Mat
img_prev
,
cv
::
Mat
img_curr
,
KeyPointsMap
&
mkps_prev
,
KeyPointsMap
&
mkps_curr
);
bool
computeEssential
(
KeyPointsMap
m
w
kps_prev
,
KeyPointsMap
m
w
kps_curr
,
TracksMap
&
tracks_prev_curr
,
cv
::
Mat
&
E
);
void
setParams
(
const
ParamsProcessorVisualOdometryPtr
_params
);
void
setParams
(
const
ParamsProcessorVisualOdometryPtr
_params
);
...
...
This diff is collapsed.
Click to expand it.
src/processor/processor_visual_odometry.cpp
+
67
−
11
View file @
ed1dcae3
...
@@ -58,6 +58,16 @@ void ProcessorVisualOdometry::configure(SensorBasePtr _sensor)
...
@@ -58,6 +58,16 @@ void ProcessorVisualOdometry::configure(SensorBasePtr _sensor)
Kcv_
=
cv
::
Mat
(
3
,
3
,
CV_32F
,
K
.
data
());
Kcv_
=
cv
::
Mat
(
3
,
3
,
CV_32F
,
K
.
data
());
}
}
TracksMap
merge_tracks
(
TracksMap
tracks_prev_curr
,
TracksMap
tracks_curr_next
){
TracksMap
tracks_prev_next
;
for
(
auto
&
match
:
tracks_prev_curr
){
if
(
tracks_curr_next
.
count
(
match
.
second
)
!=
0
){
tracks_prev_next
[
match
.
first
]
=
tracks_curr_next
[
match
.
second
];
}
}
return
tracks_prev_next
;
}
void
ProcessorVisualOdometry
::
preProcess
()
void
ProcessorVisualOdometry
::
preProcess
()
{
{
// Get Capture
// Get Capture
...
@@ -88,8 +98,10 @@ void ProcessorVisualOdometry::preProcess()
...
@@ -88,8 +98,10 @@ void ProcessorVisualOdometry::preProcess()
// TRACKING
// TRACKING
// Proceeed to tracking the previous features
// Proceeed to tracking the previous features
capture_image_last_
=
std
::
static_pointer_cast
<
CaptureImage
>
(
last_ptr_
);
capture_image_last_
=
std
::
static_pointer_cast
<
CaptureImage
>
(
last_ptr_
);
capture_image_origin_
=
std
::
static_pointer_cast
<
CaptureImage
>
(
origin_ptr_
);
cv
::
Mat
img_last
=
capture_image_last_
->
getImage
();
cv
::
Mat
img_last
=
capture_image_last_
->
getImage
();
KeyPointsMap
mwkps_origin
=
capture_image_origin_
->
getKeyPoints
();
KeyPointsMap
mwkps_last
=
capture_image_last_
->
getKeyPoints
();
KeyPointsMap
mwkps_last
=
capture_image_last_
->
getKeyPoints
();
KeyPointsMap
mwkps_incoming
;
// init incoming
KeyPointsMap
mwkps_incoming
;
// init incoming
...
@@ -103,10 +115,27 @@ void ProcessorVisualOdometry::preProcess()
...
@@ -103,10 +115,27 @@ void ProcessorVisualOdometry::preProcess()
// - ...
// - ...
////////////////////////////////
////////////////////////////////
// Tracks between last and incoming
TracksMap
tracks_last_incoming
=
kltTrack
(
img_last
,
img_incoming
,
mwkps_last
,
mwkps_incoming
);
capture_image_incoming_
->
setTracksPrev
(
tracks_last_incoming
);
// Merge tracks to get tracks_origin_incoming
TracksMap
tracks_origin_last
=
capture_image_last_
->
getTracksOrigin
();
TracksMap
tracks_origin_incoming
=
merge_tracks
(
tracks_origin_last
,
tracks_last_incoming
);
// Outliers rejection with essential matrix
cv
::
Mat
E
;
computeEssential
(
mwkps_origin
,
mwkps_incoming
,
tracks_origin_incoming
,
E
);
////////////////////////////////
////////////////////////////////
// if too few tracks left in incoming
// if too few tracks left in incoming
// detect new KeyPoints in last and track them to incoming
// detect new KeyPoints in last and track them to incoming
////////////////////////////////
////////////////////////////////
size_t
n_tracks
=
tracks_origin_incoming
.
size
();
if
(
n_tracks
<
500
){
return
;
}
////////////////////////////////
////////////////////////////////
...
@@ -190,7 +219,7 @@ unsigned int ProcessorVisualOdometry::processNew(const int& _max_features)
...
@@ -190,7 +219,7 @@ unsigned int ProcessorVisualOdometry::processNew(const int& _max_features)
std
::
cout
<<
"In incoming, track: "
<<
track_li
.
first
<<
" -> "
<<
track_li
.
second
<<
std
::
endl
;
std
::
cout
<<
"In incoming, track: "
<<
track_li
.
first
<<
" -> "
<<
track_li
.
second
<<
std
::
endl
;
if
(
!
tracks_map_li_matched_
.
count
(
track_li
.
first
)){
if
(
!
tracks_map_li_matched_
.
count
(
track_li
.
first
)){
std
::
cout
<<
"A NEW track is born!"
<<
std
::
endl
;
std
::
cout
<<
"A NEW track is born!"
<<
std
::
endl
;
// create a new last feature, a new track and add the incoming feature to this track
//
2)
create a new last feature, a new track and add the incoming feature to this track
WKeyPoint
kp_last
=
capture_image_last_
->
getKeyPoints
().
at
(
track_li
.
first
);
WKeyPoint
kp_last
=
capture_image_last_
->
getKeyPoints
().
at
(
track_li
.
first
);
FeaturePointImagePtr
feat_pi_last
=
FeatureBase
::
emplace
<
FeaturePointImage
>
(
capture_image_last_
,
kp_last
,
pixel_cov_
);
FeaturePointImagePtr
feat_pi_last
=
FeatureBase
::
emplace
<
FeaturePointImage
>
(
capture_image_last_
,
kp_last
,
pixel_cov_
);
track_matrix_
.
newTrack
(
feat_pi_last
);
track_matrix_
.
newTrack
(
feat_pi_last
);
...
@@ -223,10 +252,8 @@ void ProcessorVisualOdometry::establishFactors()
...
@@ -223,10 +252,8 @@ void ProcessorVisualOdometry::establishFactors()
for
(
auto
lmk
:
getProblem
()
->
getMap
()
->
getLandmarkList
()){
for
(
auto
lmk
:
getProblem
()
->
getMap
()
->
getLandmarkList
()){
if
(
lmk
->
id
()
==
feat_pi
->
trackId
()){
if
(
lmk
->
id
()
==
feat_pi
->
trackId
()){
associated_lmk
=
lmk
;
associated_lmk
=
lmk
;
break
;
}
}
}
}
// 1) create a factor between new KF and assocatiated track landmark
// 1) create a factor between new KF and assocatiated track landmark
// HYP: assuming the trackid are the same as the landmark ID -> BAD if other types of landmarks involved
// HYP: assuming the trackid are the same as the landmark ID -> BAD if other types of landmarks involved
if
(
associated_lmk
){
if
(
associated_lmk
){
...
@@ -236,18 +263,17 @@ void ProcessorVisualOdometry::establishFactors()
...
@@ -236,18 +263,17 @@ void ProcessorVisualOdometry::establishFactors()
// 2) create landmark if track is not associated with one and has enough length
// 2) create landmark if track is not associated with one and has enough length
else
if
(
track_matrix_
.
trackSize
(
feat
->
trackId
())
>=
min_track_length
){
else
if
(
track_matrix_
.
trackSize
(
feat
->
trackId
())
>=
min_track_length
){
std
::
cout
<<
"NEW valid track
\\
o/
-> create a new Landmark
"
<<
std
::
endl
;
std
::
cout
<<
"NEW valid track
\\
o/"
<<
std
::
endl
;
LandmarkBasePtr
lmk
=
emplaceLandmark
(
feat_pi
);
LandmarkBasePtr
lmk
=
emplaceLandmark
(
feat_pi
);
// Associate the track to the landmark
lmk
->
setId
(
feat_pi
->
trackId
());
// NOT the right way -> should be replaced by book-keeping (in the processor? Feature?)
Track
track_kf
=
track_matrix_
.
trackAtKeyframes
(
feat
->
trackId
());
// maybe call feature->setLandmarkId() on all the features of the track?
for
(
auto
feat_kf
:
track_kf
){
lmk
->
setId
(
feat_pi
->
trackId
());
LandmarkHpPtr
lmk_hp
=
std
::
dynamic_pointer_cast
<
LandmarkHp
>
(
lmk
);
LandmarkHpPtr
lmk_hp
=
std
::
dynamic_pointer_cast
<
LandmarkHp
>
(
lmk
);
for
(
auto
feat_kf
:
track_matrix_
.
trackAtKeyframes
(
feat
->
trackId
())){
FactorBase
::
emplace
<
FactorPixelHp
>
(
feat_kf
.
second
,
feat_kf
.
second
,
lmk_hp
,
shared_from_this
(),
true
);
FactorBase
::
emplace
<
FactorPixelHp
>
(
feat_kf
.
second
,
feat_kf
.
second
,
lmk_hp
,
shared_from_this
(),
true
);
}
}
}
}
}
}
}
}
LandmarkBasePtr
ProcessorVisualOdometry
::
emplaceLandmark
(
FeatureBasePtr
_feat
)
LandmarkBasePtr
ProcessorVisualOdometry
::
emplaceLandmark
(
FeatureBasePtr
_feat
)
...
@@ -286,6 +312,7 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
...
@@ -286,6 +312,7 @@ LandmarkBasePtr ProcessorVisualOdometry::emplaceLandmark(FeatureBasePtr _feat)
getSensor
(),
getSensor
(),
feat_pi
->
getKeyPoint
().
getDescriptor
());
feat_pi
->
getKeyPoint
().
getDescriptor
());
// _feat->setLandmarkId(lmk_hp_ptr->id()); // not necessary I think?
return
lmk_hp_ptr
;
return
lmk_hp_ptr
;
}
}
...
@@ -320,7 +347,7 @@ void ProcessorVisualOdometry::setParams(const ParamsProcessorVisualOdometryPtr _
...
@@ -320,7 +347,7 @@ void ProcessorVisualOdometry::setParams(const ParamsProcessorVisualOdometryPtr _
params_visual_odometry_
=
_params
;
params_visual_odometry_
=
_params
;
}
}
TracksMap
ProcessorVisualOdometry
::
klt
_t
rack
(
cv
::
Mat
img_prev
,
cv
::
Mat
img_curr
,
KeyPointsMap
&
mwkps_prev
,
KeyPointsMap
&
mwkps_curr
)
TracksMap
ProcessorVisualOdometry
::
klt
T
rack
(
cv
::
Mat
img_prev
,
cv
::
Mat
img_curr
,
KeyPointsMap
&
mwkps_prev
,
KeyPointsMap
&
mwkps_curr
)
{
{
TracksMap
tracks_prev_curr
;
TracksMap
tracks_prev_curr
;
...
@@ -386,6 +413,35 @@ TracksMap ProcessorVisualOdometry::klt_track(cv::Mat img_prev, cv::Mat img_curr,
...
@@ -386,6 +413,35 @@ TracksMap ProcessorVisualOdometry::klt_track(cv::Mat img_prev, cv::Mat img_curr,
return
tracks_prev_curr
;
return
tracks_prev_curr
;
}
}
bool
ProcessorVisualOdometry
::
computeEssential
(
KeyPointsMap
mwkps_prev
,
KeyPointsMap
mwkps_curr
,
TracksMap
&
tracks_prev_curr
,
cv
::
Mat
&
E
)
{
// We need to build lists of pt2f for openCV function
std
::
vector
<
cv
::
Point2f
>
p2f_prev
,
p2f_curr
;
std
::
vector
<
size_t
>
all_indices
;
for
(
auto
&
track
:
tracks_prev_curr
){
all_indices
.
push_back
(
track
.
first
);
p2f_prev
.
push_back
(
mwkps_prev
[
track
.
first
].
getCvKeyPoint
().
pt
);
p2f_curr
.
push_back
(
mwkps_curr
[
track
.
second
].
getCvKeyPoint
().
pt
);
}
// We need at least five tracks
if
(
p2f_prev
.
size
()
<
5
)
return
false
;
float
focal_length
=
Kcv_
.
at
<
float
>
(
0
,
0
);
cv
::
Mat
cvMask
;
cv
::
Point2f
principal_pt
(
Kcv_
.
at
<
float
>
(
0
,
2
),
Kcv_
.
at
<
float
>
(
1
,
2
));
cv
::
findEssentialMat
(
p2f_prev
,
p2f_curr
,
focal_length
,
principal_pt
,
cv
::
RANSAC
,
0.99
,
1.0
,
cvMask
);
// Let's remove outliers from tracksMap
for
(
size_t
k
=
0
;
k
<
all_indices
.
size
();
k
++
){
if
(
cvMask
.
at
<
bool
>
(
k
)
==
0
){
tracks_prev_curr
.
erase
(
all_indices
.
at
(
k
));
}
}
return
true
;
}
}
//namespace wolf
}
//namespace wolf
...
...
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