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
c804877d
Commit
c804877d
authored
4 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
reimplemented and tests added
parent
0f7ad039
No related branches found
Branches containing commit
No related tags found
Tags containing commit
1 merge request
!409
Resolve "Reimplement FrameBase::getPreviousFrame() and getNextFrame()"
Pipeline
#6314
passed
4 years ago
Stage: build
Changes
2
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/frame/frame_base.cpp
+17
-32
17 additions, 32 deletions
src/frame/frame_base.cpp
test/gtest_frame_base.cpp
+64
-3
64 additions, 3 deletions
test/gtest_frame_base.cpp
with
81 additions
and
35 deletions
src/frame/frame_base.cpp
+
17
−
32
View file @
c804877d
...
@@ -141,43 +141,28 @@ FrameBasePtr FrameBase::getPreviousFrame() const
...
@@ -141,43 +141,28 @@ FrameBasePtr FrameBase::getPreviousFrame() const
{
{
assert
(
getTrajectory
()
!=
nullptr
&&
"This Frame is not linked to any trajectory"
);
assert
(
getTrajectory
()
!=
nullptr
&&
"This Frame is not linked to any trajectory"
);
//look for the position of this node in the upper list (frame list of trajectory)
auto
current_frame_it
=
getTrajectory
()
->
getFrameMap
().
find
(
time_stamp_
);
for
(
auto
f_it
=
getTrajectory
()
->
rbegin
();
f_it
!=
getTrajectory
()
->
rend
();
f_it
++
)
{
assert
(
current_frame_it
!=
getTrajectory
()
->
getFrameMap
().
end
()
&&
"Frame not found in the frame map!"
);
if
(
this
->
frame_id_
==
(
*
f_it
)
->
id
()
)
{
if
(
current_frame_it
==
getTrajectory
()
->
getFrameMap
().
begin
())
f_it
++
;
return
nullptr
;
if
(
f_it
!=
getTrajectory
()
->
rend
())
{
return
std
::
prev
(
current_frame_it
)
->
second
;
return
*
f_it
;
}
else
{
return
nullptr
;
}
}
}
return
nullptr
;
}
}
FrameBasePtr
FrameBase
::
getNextFrame
()
const
FrameBasePtr
FrameBase
::
getNextFrame
()
const
{
{
//std::cout << "finding next frame of " << this->frame_id_ << std::endl;
assert
(
getTrajectory
()
!=
nullptr
&&
"This Frame is not linked to any trajectory"
);
auto
f_it
=
getTrajectory
()
->
rbegin
();
f_it
++
;
//starting from second last frame
//look for the position of this node in the frame list of trajectory
auto
current_frame_it
=
getTrajectory
()
->
getFrameMap
().
find
(
time_stamp_
);
while
(
f_it
!=
getTrajectory
()
->
rend
())
{
assert
(
current_frame_it
!=
getTrajectory
()
->
getFrameMap
().
end
()
&&
"Frame not found in the frame map!"
);
if
(
this
->
frame_id_
==
(
*
f_it
)
->
id
())
{
if
(
std
::
next
(
current_frame_it
)
==
getTrajectory
()
->
getFrameMap
().
end
())
f_it
--
;
return
nullptr
;
return
*
f_it
;
}
return
std
::
next
(
current_frame_it
)
->
second
;
f_it
++
;
}
std
::
cout
<<
"next frame not found!"
<<
std
::
endl
;
return
nullptr
;
}
}
CaptureBasePtr
FrameBase
::
addCapture
(
CaptureBasePtr
_capt_ptr
)
CaptureBasePtr
FrameBase
::
addCapture
(
CaptureBasePtr
_capt_ptr
)
...
...
This diff is collapsed.
Click to expand it.
test/gtest_frame_base.cpp
+
64
−
3
View file @
c804877d
...
@@ -80,9 +80,6 @@ TEST(FrameBase, LinksToTree)
...
@@ -80,9 +80,6 @@ TEST(FrameBase, LinksToTree)
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
//TODO: WARNING! I dropped this comprovations since the emplacing operation is now atomic.
ASSERT_FALSE
(
F2
->
getConstrainedByList
().
empty
());
ASSERT_FALSE
(
F2
->
getConstrainedByList
().
empty
());
// tree is inconsistent since we are missing the constrained_by link
// ASSERT_FALSE(P->check(0));
// tree is now consistent
// tree is now consistent
ASSERT_TRUE
(
P
->
check
(
0
));
ASSERT_TRUE
(
P
->
check
(
0
));
...
@@ -113,6 +110,70 @@ TEST(FrameBase, LinksToTree)
...
@@ -113,6 +110,70 @@ TEST(FrameBase, LinksToTree)
ASSERT_TRUE
(
F1
->
isFixed
());
ASSERT_TRUE
(
F1
->
isFixed
());
}
}
TEST
(
FrameBase
,
Frames
)
{
// Problem with 10 frames
ProblemPtr
P
=
Problem
::
create
(
"PO"
,
2
);
TrajectoryBasePtr
T
=
P
->
getTrajectory
();
ParamsSensorOdom2d
intrinsics_odo
;
intrinsics_odo
.
k_disp_to_disp
=
1
;
intrinsics_odo
.
k_rot_to_rot
=
1
;
auto
S
=
SensorBase
::
emplace
<
SensorOdom2d
>
(
P
->
getHardware
(),
Vector3d
::
Zero
(),
intrinsics_odo
);
auto
F0
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
0
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F1
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
1
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F2
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
2
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F3
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
3
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F4
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
4
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F5
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
5
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F6
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
6
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F7
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
7
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F8
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
8
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
auto
F9
=
FrameBase
::
emplace
<
FrameBase
>
(
T
,
9
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
// tree is now consistent
ASSERT_TRUE
(
P
->
check
(
0
));
// First and last
ASSERT_EQ
(
F0
,
T
->
getFirstFrame
());
ASSERT_EQ
(
F9
,
T
->
getLastFrame
());
// Find by timestamp
ASSERT_EQ
(
F0
,
T
->
closestFrameToTimeStamp
(
0
));
ASSERT_EQ
(
F1
,
T
->
closestFrameToTimeStamp
(
1
));
ASSERT_EQ
(
F2
,
T
->
closestFrameToTimeStamp
(
2
));
ASSERT_EQ
(
F3
,
T
->
closestFrameToTimeStamp
(
3
));
ASSERT_EQ
(
F4
,
T
->
closestFrameToTimeStamp
(
4
));
ASSERT_EQ
(
F5
,
T
->
closestFrameToTimeStamp
(
5
));
ASSERT_EQ
(
F6
,
T
->
closestFrameToTimeStamp
(
6
));
ASSERT_EQ
(
F7
,
T
->
closestFrameToTimeStamp
(
7
));
ASSERT_EQ
(
F8
,
T
->
closestFrameToTimeStamp
(
8
));
ASSERT_EQ
(
F9
,
T
->
closestFrameToTimeStamp
(
9
));
// Next frame
ASSERT_EQ
(
F1
,
F0
->
getNextFrame
());
ASSERT_EQ
(
F2
,
F1
->
getNextFrame
());
ASSERT_EQ
(
F3
,
F2
->
getNextFrame
());
ASSERT_EQ
(
F4
,
F3
->
getNextFrame
());
ASSERT_EQ
(
F5
,
F4
->
getNextFrame
());
ASSERT_EQ
(
F6
,
F5
->
getNextFrame
());
ASSERT_EQ
(
F7
,
F6
->
getNextFrame
());
ASSERT_EQ
(
F8
,
F7
->
getNextFrame
());
ASSERT_EQ
(
F9
,
F8
->
getNextFrame
());
ASSERT_EQ
(
nullptr
,
F9
->
getNextFrame
());
// Prev frame
ASSERT_EQ
(
nullptr
,
F0
->
getPreviousFrame
());
ASSERT_EQ
(
F0
,
F1
->
getPreviousFrame
());
ASSERT_EQ
(
F1
,
F2
->
getPreviousFrame
());
ASSERT_EQ
(
F2
,
F3
->
getPreviousFrame
());
ASSERT_EQ
(
F3
,
F4
->
getPreviousFrame
());
ASSERT_EQ
(
F4
,
F5
->
getPreviousFrame
());
ASSERT_EQ
(
F5
,
F6
->
getPreviousFrame
());
ASSERT_EQ
(
F6
,
F7
->
getPreviousFrame
());
ASSERT_EQ
(
F7
,
F8
->
getPreviousFrame
());
ASSERT_EQ
(
F8
,
F9
->
getPreviousFrame
());
}
#include
"core/state_block/state_quaternion.h"
#include
"core/state_block/state_quaternion.h"
TEST
(
FrameBase
,
GetSetState
)
TEST
(
FrameBase
,
GetSetState
)
{
{
...
...
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