Skip to content
GitLab
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
objectslam
Commits
2777d264
Commit
2777d264
authored
Jun 29, 2022
by
Mederic Fourmy
Browse files
Merge branch 'devel' into 3-adapt-to-state-derived-in-core
parents
fd80859d
b9b3d3a7
Changes
3
Expand all
Hide whitespace changes
Inline
Side-by-side
include/objectslam/processor/processor_tracker_landmark_object.h
View file @
2777d264
...
...
@@ -142,8 +142,26 @@ class ProcessorTrackerLandmarkObject : public ProcessorTrackerLandmark
FeatureBasePtrList
&
_features_out_incoming
);
static
std
::
vector
<
std
::
pair
<
int
,
int
>
>
allMatchesSameType
(
const
FeatureBasePtrList
&
_features_out_last
,
const
FeatureBasePtrList
&
_features_out_incoming
);
static
void
keepInliers
(
const
std
::
vector
<
Eigen
::
Isometry3d
>&
_cl_M_o_vec
,
const
std
::
vector
<
Eigen
::
Isometry3d
>&
_ci_M_o_vec
,
std
::
vector
<
std
::
pair
<
int
,
int
>
>&
matches
,
Eigen
::
Isometry3d
&
_best_model
);
/** \brief Method for create all pair of features (same type) from last and incoming
* \param _features_out_last returned list of features found in \b _capture corresponding to a landmark of _landmarks_in
* \param _features_out_incoming returned list of features found in incoming capture corresponding to a landmark of _landmarks_in
* \param _cl_M_o_vec vector of camera to object isometry for last capture
* \param _ci_M_o_vec vector of camera to object isometry for incoming capture
*
*
* \return All matches available for those two lists
*/
static
std
::
vector
<
std
::
pair
<
int
,
int
>
>
createAllMatchesSameType
(
const
FeatureBasePtrList
&
_features_out_last
,
const
FeatureBasePtrList
&
_features_out_incoming
,
std
::
vector
<
Eigen
::
Isometry3d
>&
_cl_M_o_vec
,
std
::
vector
<
Eigen
::
Isometry3d
>&
_ci_M_o_vec
);
/** \brief Method for matching feature from last to incoming
...
...
src/processor/processor_tracker_landmark_object.cpp
View file @
2777d264
...
...
@@ -511,14 +511,39 @@ std::ostream& operator<<(std::ostream &flux, std::vector<int> vector)
return
flux
;
}
std
::
vector
<
std
::
pair
<
int
,
int
>
>
ProcessorTrackerLandmarkObject
::
allMatchesSameType
(
const
FeatureBasePtrList
&
_features_out_last
,
const
FeatureBasePtrList
&
_features_out_incoming
)
void
ProcessorTrackerLandmarkObject
::
keepInliers
(
const
std
::
vector
<
Eigen
::
Isometry3d
>&
_cl_M_o_vec
,
const
std
::
vector
<
Eigen
::
Isometry3d
>&
_ci_M_o_vec
,
std
::
vector
<
std
::
pair
<
int
,
int
>
>&
matches
,
Eigen
::
Isometry3d
&
_best_model
)
{
//Vector that will store inliers/outliers pairs
std
::
vector
<
std
::
pair
<
int
,
int
>
>
inliers_idx
;
std
::
vector
<
std
::
pair
<
int
,
int
>
>
outliers_idx
;
bool
RANSACWorks
=
ProcessorTrackerLandmarkObject
::
matchingRANSAC
(
_cl_M_o_vec
,
_ci_M_o_vec
,
matches
,
0.10
,
inliers_idx
,
outliers_idx
,
_best_model
);
if
(
RANSACWorks
)
{
std
::
cout
<<
"RANSAC has worked"
<<
std
::
endl
;
//Keep only inliers
ProcessorTrackerLandmarkObject
::
filterMatchesOutliers
(
outliers_idx
,
matches
);
}
}
std
::
vector
<
std
::
pair
<
int
,
int
>
>
ProcessorTrackerLandmarkObject
::
createAllMatchesSameType
(
const
FeatureBasePtrList
&
_features_out_last
,
const
FeatureBasePtrList
&
_features_out_incoming
,
std
::
vector
<
Eigen
::
Isometry3d
>&
_cl_M_o_vec
,
std
::
vector
<
Eigen
::
Isometry3d
>&
_ci_M_o_vec
)
{
std
::
vector
<
Eigen
::
Isometry3d
>
cl_M_o_vec
;
std
::
vector
<
Eigen
::
Isometry3d
>
ci_M_o_vec
;
std
::
vector
<
std
::
pair
<
int
,
int
>
>
matches
;
//Vector that store all matches available
std
::
vector
<
std
::
pair
<
int
,
int
>
>
all_matches
;
int
index_last
=
0
;
//First pass throught the loop
bool
pushBack
=
true
;
for
(
auto
feat_last
:
_features_out_last
)
...
...
@@ -528,60 +553,43 @@ std::vector<std::pair<int,int> > ProcessorTrackerLandmarkObject::allMatchesSameT
//camera to feat (last)
Vector3d
pos_obj_last
=
feat_last
->
getMeasurement
().
head
(
3
);
Quaterniond
quat_obj_last
(
feat_last
->
getMeasurement
().
tail
(
4
).
data
());
Eigen
::
Isometry3d
cl_M_o
=
Eigen
::
Translation
<
double
,
3
>
(
pos_obj_last
)
*
quat_obj_last
;
cl_M_o_vec
.
push_back
(
cl_M_o
);
_
cl_M_o_vec
.
push_back
(
cl_M_o
);
int
index_incoming
=
0
;
for
(
auto
feat_incoming
:
_features_out_incoming
)
{
std
::
string
object_type_incoming
=
std
::
static_pointer_cast
<
FeatureObject
>
(
feat_incoming
)
->
getObjectType
();
//Create a pair only if features have same types
if
(
object_type_last
==
object_type_incoming
)
{
std
::
pair
<
int
,
int
>
pair_indexes
=
std
::
make_pair
(
index_last
,
index_incoming
);
matches
.
push_back
(
pair_indexes
);
all_
matches
.
push_back
(
pair_indexes
);
}
//
camera to feat
//
if first pass throught the loop
if
(
pushBack
)
{
//camera to feat (incoming)
Vector3d
pos_obj_incoming
=
feat_incoming
->
getMeasurement
().
head
(
3
);
Quaterniond
quat_obj_incoming
(
feat_incoming
->
getMeasurement
().
tail
(
4
).
data
());
Eigen
::
Isometry3d
ci_M_o
=
Eigen
::
Translation
<
double
,
3
>
(
pos_obj_incoming
)
*
quat_obj_incoming
;
ci_M_o_vec
.
push_back
(
ci_M_o
);
_ci_M_o_vec
.
push_back
(
ci_M_o
);
}
index_incoming
++
;
}
index_last
++
;
pushBack
=
false
;
//Not first pass throught the loop
pushBack
=
false
;
}
// std::cout << matches << "\n";
std
::
vector
<
std
::
pair
<
int
,
int
>
>
inliers_idx
;
std
::
vector
<
std
::
pair
<
int
,
int
>
>
outliers_idx
;
Eigen
::
Isometry3d
best_model
=
Eigen
::
Isometry3d
::
Identity
();
bool
RANSACWorks
=
ProcessorTrackerLandmarkObject
::
matchingRANSAC
(
cl_M_o_vec
,
ci_M_o_vec
,
matches
,
0.10
,
inliers_idx
,
outliers_idx
,
best_model
);
if
(
RANSACWorks
)
{
std
::
cout
<<
"RANSAC has worked"
<<
std
::
endl
;
//Keep only inliers
ProcessorTrackerLandmarkObject
::
filterMatchesOutliers
(
outliers_idx
,
matches
);
}
return
matches
;
return
all_matches
;
}
...
...
@@ -766,8 +774,10 @@ bool ProcessorTrackerLandmarkObject::matchingRANSAC(const std::vector<Eigen::Iso
int
nb_inliers
=
_inliers_idx
.
size
();
int
nb_outliers
=
_outliers_idx
.
size
();
if
((
double
)
nb_outliers
/
nb_inliers
>
_ratio_outliers_inliers
)
if
((
double
)
nb_outliers
/
nb_inliers
>
_ratio_outliers_inliers
)
{
return
true
;
}
return
true
;
}
...
...
test/gtest_processor_tracker_landmark_object.cpp
View file @
2777d264
This diff is collapsed.
Click to expand it.
Write
Preview
Supports
Markdown
0%
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!
Cancel
Please
register
or
sign in
to comment