Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
L
laser
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
laser
Commits
8ec2a3e3
Commit
8ec2a3e3
authored
5 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
points of polyline in state_block_vector_
parent
44669449
No related branches found
Branches containing commit
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
include/laser/landmark/landmark_polyline_2D.h
+2
-2
2 additions, 2 deletions
include/laser/landmark/landmark_polyline_2D.h
src/landmark/landmark_polyline_2D.cpp
+52
-35
52 additions, 35 deletions
src/landmark/landmark_polyline_2D.cpp
with
54 additions
and
37 deletions
include/laser/landmark/landmark_polyline_2D.h
+
2
−
2
View file @
8ec2a3e3
...
...
@@ -200,8 +200,8 @@ class LandmarkPolyline2D : public LandmarkBase
/** \brief Adds all stateBlocks of the frame to the wolfProblem list of new stateBlocks
**/
virtual
void
registerNewStateBlocks
();
virtual
void
removeStateBlocks
();
//
virtual void registerNewStateBlocks();
//
virtual void removeStateBlocks();
/** Factory method to create new landmarks from YAML nodes
*/
...
...
This diff is collapsed.
Click to expand it.
src/landmark/landmark_polyline_2D.cpp
+
52
−
35
View file @
8ec2a3e3
...
...
@@ -24,13 +24,23 @@ LandmarkPolyline2D::LandmarkPolyline2D(StateBlockPtr _p_ptr, StateBlockPtr _o_pt
}
LandmarkPolyline2D
::
LandmarkPolyline2D
(
StateBlockPtr
_p_ptr
,
StateBlockPtr
_o_ptr
,
const
Eigen
::
MatrixXs
&
_points
,
const
bool
_first_defined
,
const
bool
_last_defined
,
unsigned
int
_first_id
,
PolylineRectangularClass
_class
)
:
LandmarkBase
(
"POLYLINE 2D"
,
_p_ptr
,
_o_ptr
),
first_defined_
(
_first_defined
),
last_defined_
(
_last_defined
),
closed_
(
false
),
classification_
(
_class
),
version_
(
0
),
merged_in_lmk_
(
nullptr
)
LandmarkBase
(
"POLYLINE 2D"
,
_p_ptr
,
_o_ptr
),
first_defined_
(
_first_defined
),
last_defined_
(
_last_defined
),
closed_
(
false
),
classification_
(
_class
),
version_
(
0
),
merged_in_lmk_
(
nullptr
)
{
//std::cout << "LandmarkPolyline2D::LandmarkPolyline2D" << std::endl;
assert
(
_points
.
cols
()
>=
2
&&
"2 points at least needed."
);
for
(
auto
i
=
0
;
i
<
_points
.
cols
();
i
++
)
point_state_ptr_map_
[
i
+
_first_id
]
=
std
::
make_shared
<
StateBlock
>
(
_points
.
col
(
i
).
head
<
2
>
());
{
auto
new_sb
=
std
::
make_shared
<
StateBlock
>
(
_points
.
col
(
i
).
head
<
2
>
());
point_state_ptr_map_
[
i
+
_first_id
]
=
new_sb
;
getStateBlockVec
().
push_back
(
new_sb
);
}
if
(
!
first_defined_
)
firstStateBlock
()
->
setLocalParametrization
(
std
::
make_shared
<
LocalParametrizationPolylineExtreme
>
(
std
::
next
(
point_state_ptr_map_
.
begin
())
->
second
));
...
...
@@ -129,8 +139,6 @@ Eigen::MatrixXs LandmarkPolyline2D::computePointsGlobal(const int& _from_id, con
return
points_global
;
}
Eigen
::
MatrixXs
LandmarkPolyline2D
::
computePointsCovGlobal
()
const
{
Eigen
::
MatrixXs
points_cov_global
=
Eigen
::
MatrixXs
::
Zero
(
2
,
2
*
getNPoints
());
...
...
@@ -167,6 +175,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
std
::
make_shared
<
LocalParametrizationPolylineExtreme
>
(
lastStateBlock
())
:
nullptr
));
point_state_ptr_map_
[
getLastId
()
+
1
]
=
new_sb_ptr
;
getStateBlockVec
().
push_back
(
new_sb_ptr
);
if
(
getProblem
())
getProblem
()
->
notifyStateBlock
(
new_sb_ptr
,
ADD
);
...
...
@@ -181,6 +190,7 @@ void LandmarkPolyline2D::addPoint(const Eigen::VectorXs& _point, const bool& _de
std
::
make_shared
<
LocalParametrizationPolylineExtreme
>
(
firstStateBlock
())
:
nullptr
));
point_state_ptr_map_
[
getFirstId
()
-
1
]
=
new_sb_ptr
;
getStateBlockVec
().
push_back
(
new_sb_ptr
);
if
(
getProblem
())
getProblem
()
->
notifyStateBlock
(
new_sb_ptr
,
ADD
);
...
...
@@ -216,6 +226,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
std
::
make_shared
<
LocalParametrizationPolylineExtreme
>
(
lastStateBlock
())
:
nullptr
));
point_state_ptr_map_
[
getLastId
()
+
1
]
=
new_sb_ptr
;
getStateBlockVec
().
push_back
(
new_sb_ptr
);
if
(
getProblem
())
getProblem
()
->
notifyStateBlock
(
new_sb_ptr
,
ADD
);
...
...
@@ -232,6 +243,7 @@ void LandmarkPolyline2D::addPoints(const Eigen::MatrixXs& _points, const unsigne
std
::
make_shared
<
LocalParametrizationPolylineExtreme
>
(
firstStateBlock
())
:
nullptr
));
point_state_ptr_map_
[
getFirstId
()
-
1
]
=
new_sb_ptr
;
getStateBlockVec
().
push_back
(
new_sb_ptr
);
if
(
getProblem
())
getProblem
()
->
notifyStateBlock
(
new_sb_ptr
,
ADD
);
...
...
@@ -291,7 +303,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
return
false
;
}
WOLF_
TRACE
(
"tryClose landmark "
,
id
(),
" point ids: "
);
WOLF_
DEBUG
(
"tryClose landmark "
,
id
(),
" point ids: "
);
for
(
auto
id
:
getValidIds
())
std
::
cout
<<
id
<<
" "
;
std
::
cout
<<
std
::
endl
;
...
...
@@ -311,7 +323,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// last_with_id is the point overlapped with the last defined point, it CAN'T be overlapped with with_point_id since it's already overlapped with first defined point
while
(
with_point_id
>
last_with_id
)
{
WOLF_
TRACE
(
"first_point_id: "
,
first_point_id
,
"
\n
with_point_id: "
,
with_point_id
,
"
\n
last_with_id: "
,
last_with_id
);
WOLF_
DEBUG
(
"first_point_id: "
,
first_point_id
,
"
\n
with_point_id: "
,
with_point_id
,
"
\n
last_with_id: "
,
last_with_id
);
if
((
getPointVector
(
with_point_id
)
-
getPointVector
(
first_point_id
)).
squaredNorm
()
<
_dist_tol
*
_dist_tol
)
{
found
=
true
;
...
...
@@ -325,7 +337,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// IF MATCH FOUND CHECK THE WHOLE OVERLAPPING
if
(
found
)
{
WOLF_
TRACE
(
"good point match first: "
,
first_point_id
,
" with: "
,
with_point_id
);
WOLF_
DEBUG
(
"good point match first: "
,
first_point_id
,
" with: "
,
with_point_id
);
// COMPUTE OVERLAPPING POINTS
// if first not defined -> add previous overlapping point
...
...
@@ -368,12 +380,12 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
if
(
sq_dist
>
_dist_tol
*
_dist_tol
)
{
WOLF_
TRACE
(
"
\n
Bad overlapping: "
,
sq_dist
,
WOLF_
DEBUG
(
"
\n
Bad overlapping: "
,
sq_dist
,
"
\n\t
point
\t
"
,
overlapped_ids
[
i
].
first
,
" ("
,
getPointVector
(
overlapped_ids
[
i
].
first
).
transpose
(),
")"
,
(
overlapped_ids
[
i
].
first
==
getFirstId
()
&&
!
first_defined_
?
"NOT DEFINED"
:
""
),
"
\n\t
with
\t
"
,
overlapped_ids
[
i
].
second
,
" ("
,
getPointVector
(
overlapped_ids
[
i
].
second
).
transpose
(),
")"
,
(
overlapped_ids
[
i
].
second
==
getLastId
()
&&
!
last_defined_
?
"NOT DEFINED"
:
""
));
return
false
;
}
WOLF_
TRACE
(
"
\n
Good overlapping: "
,
sq_dist
,
WOLF_
DEBUG
(
"
\n
Good overlapping: "
,
sq_dist
,
"
\n\t
point
\t
"
,
overlapped_ids
[
i
].
first
,
" ("
,
getPointVector
(
overlapped_ids
[
i
].
first
).
transpose
(),
")"
,
(
overlapped_ids
[
i
].
first
==
getFirstId
()
&&
!
first_defined_
?
"NOT DEFINED"
:
""
),
"
\n\t
with
\t
"
,
overlapped_ids
[
i
].
second
,
" ("
,
getPointVector
(
overlapped_ids
[
i
].
second
).
transpose
(),
")"
,
(
overlapped_ids
[
i
].
second
==
getLastId
()
&&
!
last_defined_
?
"NOT DEFINED"
:
""
));
}
...
...
@@ -384,7 +396,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// Trying to close overlapping both not defined extremes
if
(
overlapped_ids
.
empty
()
&&
!
first_defined_
&&
!
last_defined_
)
{
WOLF_
TRACE
(
"No overlapping found with defined points, trying with not defined extremes"
);
WOLF_
DEBUG
(
"No overlapping found with defined points, trying with not defined extremes"
);
// Check for both not defined extremes that the distance to the segment of first and last defined points is below the threshold
Eigen
::
Vector2s
first_not_def
=
getPointVector
(
getFirstId
());
...
...
@@ -395,7 +407,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
if
(
sqDistPoint2Segment
(
first_not_def
,
last_def
,
first_def
)
<
_dist_tol
*
_dist_tol
&&
sqDistPoint2Segment
(
last_not_def
,
last_def
,
first_def
)
<
_dist_tol
*
_dist_tol
)
{
WOLF_
TRACE
(
"The two not defined extremes close to the segment defined first_defined and last_defined:"
,
WOLF_
DEBUG
(
"The two not defined extremes close to the segment defined first_defined and last_defined:"
,
"
\n\t
first point: "
,
first_not_def
.
transpose
(),
"
\n\t
last point: "
,
last_not_def
.
transpose
(),
"
\n\t
first defined point: "
,
first_def
.
transpose
(),
...
...
@@ -409,7 +421,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
if
(
last_proj_2_first_def
<
first_proj_2_first_def
)
{
WOLF_
TRACE
(
"The not defined extremes are overlapped"
,
WOLF_
DEBUG
(
"The not defined extremes are overlapped"
,
"
\n\t
last_proj_2_first_def: "
,
last_proj_2_first_def
,
"
\n\t
first_proj_2_first_def: "
,
first_proj_2_first_def
);
overlapped_ids
.
push_back
(
std
::
pair
<
int
,
int
>
(
getFirstId
(),
getLastId
()));
...
...
@@ -420,7 +432,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// IF ANY OVERLAPPING -> CLOSE AND MERGE OVERLAPPED POINTS
if
(
!
overlapped_ids
.
empty
())
{
WOLF_
TRACE
(
"All overlappings OK, merging "
,
overlapped_ids
.
size
(),
" points"
);
WOLF_
DEBUG
(
"All overlappings OK, merging "
,
overlapped_ids
.
size
(),
" points"
);
// merge points
while
(
!
overlapped_ids
.
empty
())
...
...
@@ -435,7 +447,7 @@ bool LandmarkPolyline2D::tryClose(const Scalar& _dist_tol)
// Close
setClosed
();
WOLF_
TRACE
(
"Landmark "
,
id
(),
" was closed."
);
WOLF_
DEBUG
(
"Landmark "
,
id
(),
" was closed."
);
return
true
;
}
...
...
@@ -543,8 +555,13 @@ void LandmarkPolyline2D::mergePoints(int _remove_id, int _remain_id)
getProblem
()
->
notifyStateBlock
(
remove_state
,
REMOVE
);
//std::cout << "state removed " << std::endl;
// remove
element
from
deque
// remove
stateblock
from
map
point_state_ptr_map_
.
erase
(
_remove_id
);
// remove state block from state_block_vec
auto
sb_it
=
std
::
find
(
getStateBlockVec
().
begin
(),
getStateBlockVec
().
end
(),
remove_state
);
assert
(
sb_it
!=
getStateBlockVec
().
end
());
getStateBlockVec
().
erase
(
sb_it
);
//std::cout << "state removed from point vector " << std::endl;
// new version
...
...
@@ -907,26 +924,26 @@ void LandmarkPolyline2D::mergeLandmark(const LandmarkPolyline2DPtr _merged_lmk,
//std::cout << "\tLandmark deleted\n";
}
void
LandmarkPolyline2D
::
registerNewStateBlocks
()
{
LandmarkBase
::
registerNewStateBlocks
();
if
(
getProblem
())
for
(
auto
state_it
:
point_state_ptr_map_
)
getProblem
()
->
notifyStateBlock
(
state_it
.
second
,
ADD
);
}
void
LandmarkPolyline2D
::
removeStateBlocks
()
{
auto
sbp_pair
=
point_state_ptr_map_
.
begin
();
while
(
sbp_pair
!=
point_state_ptr_map_
.
end
())
{
if
(
getProblem
())
getProblem
()
->
notifyStateBlock
(
sbp_pair
->
second
,
REMOVE
);
sbp_pair
=
point_state_ptr_map_
.
erase
(
sbp_pair
);
}
LandmarkBase
::
removeStateBlocks
();
}
//
void LandmarkPolyline2D::registerNewStateBlocks()
//
{
//
LandmarkBase::registerNewStateBlocks();
//
if (getProblem())
//
for (auto state_it : point_state_ptr_map_)
//
getProblem()->notifyStateBlock(state_it.second, ADD);
//
}
//
void LandmarkPolyline2D::removeStateBlocks()
//
{
//
auto sbp_pair = point_state_ptr_map_.begin();
//
while (sbp_pair != point_state_ptr_map_.end())
//
{
//
if (getProblem())
//
getProblem()->notifyStateBlock(sbp_pair->second, REMOVE);
//
//
sbp_pair = point_state_ptr_map_.erase(sbp_pair);
//
}
//
LandmarkBase::removeStateBlocks();
//
}
// static
LandmarkBasePtr
LandmarkPolyline2D
::
create
(
const
YAML
::
Node
&
_lmk_node
)
...
...
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