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
a34bb175
Commit
a34bb175
authored
7 years ago
by
Joan Solà Ortega
Browse files
Options
Downloads
Patches
Plain Diff
WIP Start migration to new state machine ...
Code not called yet
parent
0ee37e0b
No related branches found
No related tags found
1 merge request
!157
Kfpackmanager
Changes
1
Hide whitespace changes
Inline
Side-by-side
Showing
1 changed file
src/processor_motion.cpp
+99
-28
99 additions, 28 deletions
src/processor_motion.cpp
with
99 additions
and
28 deletions
src/processor_motion.cpp
+
99
−
28
View file @
a34bb175
...
...
@@ -56,44 +56,111 @@ void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr)
switch
(
processing_step_
)
{
case
FIRST_TIME_WITH_PACK
:
{
KFPackPtr
pack
=
selectPack
(
incoming_ptr_
);
setOrigin
(
pack
->
key_frame
);
// TODO process
break
;
}
case
FIRST_TIME_WITHOUT_PACK
:
{
// Create new KF for origin
std
::
cout
<<
"PM: make KF"
<<
std
::
endl
;
VectorXs
x0
=
getProblem
()
->
zeroState
();
setOrigin
(
x0
,
_incoming_ptr
->
getTimeStamp
());
break
;
}
case
SECOND_TIME_WITH_PACK
:
{
KFPackPtr
pack
=
selectPack
(
last_ptr_
);
break
;
}
case
SECOND_TIME_WITHOUT_PACK
:
{
break
;
WOLF_WARN
(
"ProcessorMotion received data before being initialized."
);
WOLF_INFO
(
"Did you forget to issue a Problem::setPrior()?"
);
throw
std
::
runtime_error
(
"ProcessorMotion received data before being initialized."
);
}
case
RUNNING_WITH_PACK
:
{
////////////////////////////////////////////////////
// FIRST open pack and join KF
KFPackPtr
pack
=
selectPack
(
last_ptr_
);
kf_pack_buffer_
.
removeUpTo
(
last_ptr_
->
getTimeStamp
()
);
// extract pack elements TODO rename _new_keyframe
FrameBasePtr
new_kf
=
pack
->
key_frame
;
TimeStamp
new_ts
=
new_kf
->
getTimeStamp
();
// find the capture whose buffer is affected by the new keyframe
auto
existing_capture
=
findCaptureContainingTimeStamp
(
new_ts
);
if
(
existing_capture
==
nullptr
)
// Keyframe without Capture --> first time
{
// TODO should be an error
}
else
// Normal operation
{
// Find the frame acting as the capture's origin
auto
keyframe_origin
=
existing_capture
->
getOriginFramePtr
();
// emplace a new motion capture to the new keyframe
auto
new_capture
=
emplaceCapture
(
new_kf
,
getSensorPtr
(),
new_ts
,
Eigen
::
VectorXs
::
Zero
(
data_size_
),
existing_capture
->
getDataCovariance
(),
existing_capture
->
getCalibration
(),
existing_capture
->
getCalibration
(),
keyframe_origin
);
// split the buffer
// and give the part of the buffer before the new keyframe to the key_capture
existing_capture
->
getBuffer
().
split
(
new_ts
,
new_capture
->
getBuffer
());
// TODO add interpolation code ...
// create motion feature and add it to the capture
auto
new_feature
=
emplaceFeature
(
new_capture
);
// create motion constraint and add it to the feature, and constrain to the other capture (origin)
emplaceConstraint
(
new_feature
,
keyframe_origin
->
getCaptureOf
(
getSensorPtr
())
);
// Update the processor pointers
if
(
existing_capture
==
last_ptr_
)
// reset processor origin
origin_ptr_
=
new_capture
;
// Update the existing capture
existing_capture
->
setOriginFramePtr
(
new_kf
);
// re-integrate existing buffer -- note: the result of re-integration is stored in the same buffer!
reintegrateBuffer
(
existing_capture
);
// modify existing feature and constraint (if they exist in the existing capture)
if
(
!
existing_capture
->
getFeatureList
().
empty
())
{
auto
existing_feature
=
existing_capture
->
getFeatureList
().
back
();
// there is only one feature!
// Modify existing feature --------
existing_feature
->
setMeasurement
(
existing_capture
->
getBuffer
().
get
().
back
().
delta_integr_
);
existing_feature
->
setMeasurementCovariance
(
existing_capture
->
getBuffer
().
get
().
back
().
delta_integr_cov_
);
// Modify existing constraint --------
// Instead of modifying, we remove one ctr, and create a new one.
auto
ctr_to_remove
=
existing_feature
->
getConstraintList
().
back
();
// there is only one constraint!
auto
new_ctr
=
emplaceConstraint
(
existing_feature
,
new_capture
);
ctr_to_remove
->
remove
();
// remove old constraint now (otherwise c->remove() gets propagated to f, C, F, etc.)
}
}
////////////////////////////////////////////////////
// NOW on with the received data
// integrate data
integrateOneStep
();
// Update state and time stamps
last_ptr_
->
setTimeStamp
(
getCurrentTimeStamp
());
last_ptr_
->
getFramePtr
()
->
setTimeStamp
(
getCurrentTimeStamp
());
last_ptr_
->
getFramePtr
()
->
setState
(
getCurrentState
());
resetDerived
();
// TODO see where to put this
break
;
}
case
RUNNING_WITHOUT_PACK
:
{
preProcess
();
// integrate data
integrateOneStep
();
// Update state and time stamps
last_ptr_
->
setTimeStamp
(
incoming_ptr_
->
ge
tTimeStamp
());
last_ptr_
->
setTimeStamp
(
getCurren
tTimeStamp
());
last_ptr_
->
getFramePtr
()
->
setTimeStamp
(
getCurrentTimeStamp
());
last_ptr_
->
getFramePtr
()
->
setState
(
getCurrentState
());
...
...
@@ -132,20 +199,18 @@ void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr)
delta_integrated_cov_
.
setZero
();
jacobian_calib_
.
setZero
();
// reset processor origin to the new keyframe's capture
origin_ptr_
=
last_ptr_
;
last_ptr_
=
new_capture_ptr
;
// reset derived things
resetDerived
();
// Update pointers
origin_ptr_
=
last_ptr_
;
last_ptr_
=
new_capture_ptr
;
// callback to other processors
getProblem
()
->
keyFrameCallback
(
key_frame_ptr
,
shared_from_this
(),
time_tolerance_
);
}
postProcess
();
// clear incoming just in case
incoming_ptr_
=
nullptr
;
// This line is not really needed, but it makes things clearer.
...
...
@@ -154,6 +219,9 @@ void ProcessorMotion::process2(CaptureBasePtr _incoming_ptr)
default
:
break
;
}
postProcess
();
}
void
ProcessorMotion
::
process
(
CaptureBasePtr
_incoming_ptr
)
...
...
@@ -397,6 +465,9 @@ void ProcessorMotion::setOrigin(FrameBasePtr _origin_frame)
bool
ProcessorMotion
::
keyFrameCallback
(
FrameBasePtr
_new_keyframe
,
const
Scalar
&
_time_tol_other
)
{
// ProcessorBase::keyFrameCallback(_new_keyframe, _time_tol_other);
// return true;
assert
(
_new_keyframe
->
getTrajectoryPtr
()
!=
nullptr
&&
"ProcessorMotion::keyFrameCallback: key frame must be in the trajectory."
);
...
...
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