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
e6bb58bf
Commit
e6bb58bf
authored
8 years ago
by
Joan Solà Ortega
Browse files
Options
Downloads
Patches
Plain Diff
Update last_key_frame_ptr_ after sortFrame(frm)
All tests passed
parent
666b3e88
No related branches found
No related tags found
No related merge requests found
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/test/gtest_trajectory.cpp
+40
-17
40 additions, 17 deletions
src/test/gtest_trajectory.cpp
src/trajectory_base.cpp
+1
-0
1 addition, 0 deletions
src/trajectory_base.cpp
with
41 additions
and
17 deletions
src/test/gtest_trajectory.cpp
+
40
−
17
View file @
e6bb58bf
...
@@ -17,6 +17,9 @@
...
@@ -17,6 +17,9 @@
using
namespace
wolf
;
using
namespace
wolf
;
/// Set to true if you want debug info
bool
debug
=
false
;
TEST
(
TrajectoryBase
,
ClosestKeyFrame
)
TEST
(
TrajectoryBase
,
ClosestKeyFrame
)
{
{
...
@@ -24,7 +27,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
...
@@ -24,7 +27,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
TrajectoryBasePtr
T
=
P
->
getTrajectoryPtr
();
TrajectoryBasePtr
T
=
P
->
getTrajectoryPtr
();
// Trajectory status:
// Trajectory status:
//
// kf1 kf2 f3 frames
// kf1 kf2 f3 frames
// 1 2 3 time stamps
// 1 2 3 time stamps
// --+-----+-----+---> time
// --+-----+-----+---> time
...
@@ -36,8 +38,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
...
@@ -36,8 +38,6 @@ TEST(TrajectoryBase, ClosestKeyFrame)
T
->
addFrame
(
f2
);
T
->
addFrame
(
f2
);
T
->
addFrame
(
f3
);
T
->
addFrame
(
f3
);
P
->
print
();
FrameBasePtr
kf
;
// closest key-frame queried
FrameBasePtr
kf
;
// closest key-frame queried
kf
=
T
->
closestKeyFrameToTimeStamp
(
-
0.8
);
// before all keyframes --> return f0
kf
=
T
->
closestKeyFrameToTimeStamp
(
-
0.8
);
// before all keyframes --> return f0
...
@@ -64,7 +64,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
...
@@ -64,7 +64,6 @@ TEST(TrajectoryBase, Add_Remove_Frame)
TrajectoryBasePtr
T
=
P
->
getTrajectoryPtr
();
TrajectoryBasePtr
T
=
P
->
getTrajectoryPtr
();
// Trajectory status:
// Trajectory status:
//
// kf1 kf2 f3 frames
// kf1 kf2 f3 frames
// 1 2 3 time stamps
// 1 2 3 time stamps
// --+-----+-----+---> time
// --+-----+-----+---> time
...
@@ -75,19 +74,19 @@ TEST(TrajectoryBase, Add_Remove_Frame)
...
@@ -75,19 +74,19 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// add frames and keyframes
// add frames and keyframes
T
->
addFrame
(
f1
);
// KF
T
->
addFrame
(
f1
);
// KF
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
1
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
1
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
2
);
T
->
addFrame
(
f2
);
// KF
T
->
addFrame
(
f2
);
// KF
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
2
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
4
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
4
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
4
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
4
);
T
->
addFrame
(
f3
);
// F
T
->
addFrame
(
f3
);
// F
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
3
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
3
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
4
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
4
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
4
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
4
);
...
@@ -97,7 +96,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
...
@@ -97,7 +96,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
// remove frames and keyframes
// remove frames and keyframes
f2
->
remove
();
// KF
f2
->
remove
();
// KF
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
2
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
2
);
...
@@ -106,7 +105,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
...
@@ -106,7 +105,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f1
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f1
->
id
());
f3
->
remove
();
// F
f3
->
remove
();
// F
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
1
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
1
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
2
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
2
);
...
@@ -114,7 +113,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
...
@@ -114,7 +113,7 @@ TEST(TrajectoryBase, Add_Remove_Frame)
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f1
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f1
->
id
());
f1
->
remove
();
// KF
f1
->
remove
();
// KF
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
0
);
ASSERT_EQ
(
T
->
getFrameList
().
size
(),
0
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
0
);
ASSERT_EQ
(
P
->
getStateBlockList
().
size
(),
0
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
0
);
ASSERT_EQ
(
P
->
getStateBlockNotificationList
().
size
(),
0
);
...
@@ -129,7 +128,6 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
...
@@ -129,7 +128,6 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
TrajectoryBasePtr
T
=
P
->
getTrajectoryPtr
();
TrajectoryBasePtr
T
=
P
->
getTrajectoryPtr
();
// Trajectory status:
// Trajectory status:
//
// kf1 kf2 f3 frames
// kf1 kf2 f3 frames
// 1 2 3 time stamps
// 1 2 3 time stamps
// --+-----+-----+---> time
// --+-----+-----+---> time
...
@@ -140,36 +138,61 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
...
@@ -140,36 +138,61 @@ TEST(TrajectoryBase, KeyFramesAreSorted)
// add frames and keyframes in random order --> keyframes must be sorted after that
// add frames and keyframes in random order --> keyframes must be sorted after that
T
->
addFrame
(
f2
);
// KF2
T
->
addFrame
(
f2
);
// KF2
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f2
->
id
());
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f2
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f2
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f2
->
id
());
T
->
addFrame
(
f3
);
// F3
T
->
addFrame
(
f3
);
// F3
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f2
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f2
->
id
());
T
->
addFrame
(
f1
);
// KF1
T
->
addFrame
(
f1
);
// KF1
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f2
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f2
->
id
());
f3
->
setKey
();
// set KF3
f3
->
setKey
();
// set KF3
P
->
print
(
2
,
0
,
0
,
0
);
if
(
debug
)
P
->
print
(
2
,
0
,
0
,
0
);
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f3
->
id
());
FrameBasePtr
f4
=
std
::
make_shared
<
FrameBase
>
(
NON_KEY_FRAME
,
1.5
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
// non-key-frame
FrameBasePtr
f4
=
std
::
make_shared
<
FrameBase
>
(
NON_KEY_FRAME
,
1.5
,
make_shared
<
StateBlock
>
(
2
),
make_shared
<
StateBlock
>
(
1
));
// non-key-frame
T
->
addFrame
(
f4
);
T
->
addFrame
(
f4
);
P
->
print
(
2
,
0
,
1
,
0
);
// Trajectory status:
// kf1 kf2 kf3 f4 frames
// 1 2 3 1.5 time stamps
// --+-----+-----+------+---> time
if
(
debug
)
P
->
print
(
2
,
0
,
1
,
0
);
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f4
->
id
());
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f4
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f3
->
id
());
f4
->
setKey
();
f4
->
setKey
();
P
->
print
(
2
,
0
,
1
,
0
);
// Trajectory status:
// kf1 kf4 kf2 kf3 frames
// 1 1.5 2 3 time stamps
// --+-----+-----+------+---> time
if
(
debug
)
P
->
print
(
2
,
0
,
1
,
0
);
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f3
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f3
->
id
());
f2
->
setTimeStamp
(
4
);
// Trajectory status:
// kf1 kf4 kf3 kf2 frames
// 1 1.5 3 4 time stamps
// --+-----+-----+------+---> time
if
(
debug
)
P
->
print
(
2
,
0
,
1
,
0
);
ASSERT_EQ
(
T
->
getLastFramePtr
()
->
id
(),
f2
->
id
());
ASSERT_EQ
(
T
->
getLastKeyFramePtr
()
->
id
(),
f2
->
id
());
f4
->
setTimeStamp
(
0.5
);
// Trajectory status:
// kf4 kf2 kf3 kf2 frames
// 0.5 1 3 4 time stamps
// --+-----+-----+------+---> time
if
(
debug
)
P
->
print
(
2
,
0
,
1
,
0
);
ASSERT_EQ
(
T
->
getFrameList
().
front
()
->
id
(),
f4
->
id
());
}
}
int
main
(
int
argc
,
char
**
argv
)
int
main
(
int
argc
,
char
**
argv
)
...
...
This diff is collapsed.
Click to expand it.
src/trajectory_base.cpp
+
1
−
0
View file @
e6bb58bf
...
@@ -48,6 +48,7 @@ void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
...
@@ -48,6 +48,7 @@ void TrajectoryBase::getConstraintList(ConstraintBaseList & _ctr_list)
void
TrajectoryBase
::
sortFrame
(
FrameBasePtr
_frame_ptr
)
void
TrajectoryBase
::
sortFrame
(
FrameBasePtr
_frame_ptr
)
{
{
moveFrame
(
_frame_ptr
,
computeFrameOrder
(
_frame_ptr
));
moveFrame
(
_frame_ptr
,
computeFrameOrder
(
_frame_ptr
));
last_key_frame_ptr_
=
findLastKeyFramePtr
();
}
}
FrameBaseIter
TrajectoryBase
::
computeFrameOrder
(
FrameBasePtr
_frame_ptr
)
FrameBaseIter
TrajectoryBase
::
computeFrameOrder
(
FrameBasePtr
_frame_ptr
)
...
...
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