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
fda5bcfe
Commit
fda5bcfe
authored
6 years ago
by
Joan Solà Ortega
Browse files
Options
Downloads
Patches
Plain Diff
Fix some bugs in Hello-wolf
parent
d8bbbc3f
No related branches found
No related tags found
1 merge request
!213
Hello wolf
Changes
2
Hide whitespace changes
Inline
Side-by-side
Showing
2 changed files
src/frame_base.cpp
+1
-1
1 addition, 1 deletion
src/frame_base.cpp
src/hello_wolf/hello_wolf.cpp
+27
-22
27 additions, 22 deletions
src/hello_wolf/hello_wolf.cpp
with
28 additions
and
23 deletions
src/frame_base.cpp
+
1
−
1
View file @
fda5bcfe
...
@@ -14,7 +14,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
...
@@ -14,7 +14,7 @@ unsigned int FrameBase::frame_id_count_ = 0;
FrameBase
::
FrameBase
(
const
TimeStamp
&
_ts
,
StateBlockPtr
_p_ptr
,
StateBlockPtr
_o_ptr
,
StateBlockPtr
_v_ptr
)
:
FrameBase
::
FrameBase
(
const
TimeStamp
&
_ts
,
StateBlockPtr
_p_ptr
,
StateBlockPtr
_o_ptr
,
StateBlockPtr
_v_ptr
)
:
NodeBase
(
"FRAME"
,
"Base"
),
NodeBase
(
"FRAME"
,
"Base"
),
trajectory_ptr_
(),
trajectory_ptr_
(),
state_block_vec_
(
3
),
// allow for
6
state blocks by default. Resize in derived constructors if needed.
state_block_vec_
(
3
),
// allow for
3
state blocks by default. Resize in derived constructors if needed.
is_removing_
(
false
),
is_removing_
(
false
),
frame_id_
(
++
frame_id_count_
),
frame_id_
(
++
frame_id_count_
),
type_
(
NON_KEY_FRAME
),
type_
(
NON_KEY_FRAME
),
...
...
This diff is collapsed.
Click to expand it.
src/hello_wolf/hello_wolf.cpp
+
27
−
22
View file @
fda5bcfe
...
@@ -27,7 +27,7 @@
...
@@ -27,7 +27,7 @@
#include
"ceres_wrapper/ceres_manager.h"
#include
"ceres_wrapper/ceres_manager.h"
int
main
()
int
main
()
{
{
/*
/*
* ============= PROBLEM DEFINITION ==================
* ============= PROBLEM DEFINITION ==================
*
*
...
@@ -117,12 +117,12 @@ int main()
...
@@ -117,12 +117,12 @@ int main()
// sensor odometer 2D
// sensor odometer 2D
IntrinsicsOdom2DPtr
intrinsics_odo
=
std
::
make_shared
<
IntrinsicsOdom2D
>
();
IntrinsicsOdom2DPtr
intrinsics_odo
=
std
::
make_shared
<
IntrinsicsOdom2D
>
();
intrinsics_odo
->
k_disp_to_disp
=
0.1
;
intrinsics_odo
->
k_rot_to_rot
=
0.1
;
SensorBasePtr
sensor_odo
=
problem
->
installSensor
(
"ODOM 2D"
,
"sensor odo"
,
Vector3s
(
0
,
0
,
0
),
intrinsics_odo
);
SensorBasePtr
sensor_odo
=
problem
->
installSensor
(
"ODOM 2D"
,
"sensor odo"
,
Vector3s
(
0
,
0
,
0
),
intrinsics_odo
);
// processor odometer 2D
// processor odometer 2D
ProcessorParamsOdom2DPtr
params_odo
=
std
::
make_shared
<
ProcessorParamsOdom2D
>
();
ProcessorParamsOdom2DPtr
params_odo
=
std
::
make_shared
<
ProcessorParamsOdom2D
>
();
params_odo
->
voting_active
=
true
;
params_odo
->
time_tolerance
=
0.1
;
params_odo
->
max_time_span
=
999
;
params_odo
->
max_time_span
=
999
;
params_odo
->
dist_traveled
=
0.95
;
// Will make KFs automatically every 1m displacement
params_odo
->
dist_traveled
=
0.95
;
// Will make KFs automatically every 1m displacement
params_odo
->
angle_turned
=
999
;
params_odo
->
angle_turned
=
999
;
...
@@ -133,14 +133,16 @@ int main()
...
@@ -133,14 +133,16 @@ int main()
// sensor Range and Bearing
// sensor Range and Bearing
IntrinsicsRangeBearingPtr
intrinsics_rb
=
std
::
make_shared
<
IntrinsicsRangeBearing
>
();
IntrinsicsRangeBearingPtr
intrinsics_rb
=
std
::
make_shared
<
IntrinsicsRangeBearing
>
();
intrinsics_rb
->
noise_bearing_degrees_std
=
1.0
;
intrinsics_rb
->
noise_range_metres_std
=
0.1
;
intrinsics_rb
->
noise_range_metres_std
=
0.1
;
intrinsics_rb
->
noise_bearing_degrees_std
=
1.0
;
SensorBasePtr
sensor_rb
=
problem
->
installSensor
(
"RANGE BEARING"
,
"sensor RB"
,
Vector3s
(
1
,
1
,
0
),
intrinsics_rb
);
SensorBasePtr
sensor_rb
=
problem
->
installSensor
(
"RANGE BEARING"
,
"sensor RB"
,
Vector3s
(
1
,
1
,
0
),
intrinsics_rb
);
sensor_rb
->
fix
();
// NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only)
// NOTE: uncomment this line below to achieve sensor self-calibration (of the orientation only)
//
sensor_rb->getOPtr()->unfix();
sensor_rb
->
getOPtr
()
->
unfix
();
// processor Range and Bearing
// processor Range and Bearing
ProcessorParamsRangeBearingPtr
params_rb
=
std
::
make_shared
<
ProcessorParamsRangeBearing
>
();
ProcessorParamsRangeBearingPtr
params_rb
=
std
::
make_shared
<
ProcessorParamsRangeBearing
>
();
params_rb
->
voting_active
=
false
;
params_rb
->
time_tolerance
=
0.01
;
params_rb
->
time_tolerance
=
0.01
;
ProcessorBasePtr
processor_rb
=
problem
->
installProcessor
(
"RANGE BEARING"
,
"processor RB"
,
sensor_rb
,
params_rb
);
ProcessorBasePtr
processor_rb
=
problem
->
installProcessor
(
"RANGE BEARING"
,
"processor RB"
,
sensor_rb
,
params_rb
);
...
@@ -164,10 +166,10 @@ int main()
...
@@ -164,10 +166,10 @@ int main()
// STEP 1 --------------------------------------------------------------
// STEP 1 --------------------------------------------------------------
// initialize
// initialize
TimeStamp
t
(
0.0
);
TimeStamp
t
(
0.0
);
// t : 0.0
Vector3s
x
(
0
,
0
,
0
);
Vector3s
x
(
0
,
0
,
0
);
Matrix3s
P
=
Matrix3s
::
Identity
()
*
0.1
;
Matrix3s
P
=
Matrix3s
::
Identity
()
*
0.1
;
problem
->
setPrior
(
x
,
P
,
t
,
0.5
);
// KF1
problem
->
setPrior
(
x
,
P
,
t
,
0.5
);
// KF1
: (0,0,0)
// observe lmks
// observe lmks
ids
.
resize
(
1
);
ranges
.
resize
(
1
);
bearings
.
resize
(
1
);
ids
.
resize
(
1
);
ranges
.
resize
(
1
);
bearings
.
resize
(
1
);
...
@@ -175,14 +177,14 @@ int main()
...
@@ -175,14 +177,14 @@ int main()
ranges
<<
1.0
;
// see drawing
ranges
<<
1.0
;
// see drawing
bearings
<<
M_PI
/
2
;
bearings
<<
M_PI
/
2
;
CaptureRangeBearingPtr
cap_rb
=
std
::
make_shared
<
CaptureRangeBearing
>
(
t
,
sensor_rb
,
ids
,
ranges
,
bearings
);
CaptureRangeBearingPtr
cap_rb
=
std
::
make_shared
<
CaptureRangeBearing
>
(
t
,
sensor_rb
,
ids
,
ranges
,
bearings
);
sensor_rb
->
process
(
cap_rb
);
sensor_rb
->
process
(
cap_rb
);
// L1 : (1,2)
// STEP 2 --------------------------------------------------------------
// STEP 2 --------------------------------------------------------------
t
+=
1.0
;
t
+=
1.0
;
// t : 1.0
// motion
// motion
CaptureOdom2DPtr
cap_motion
=
std
::
make_shared
<
CaptureOdom2D
>
(
t
,
sensor_odo
,
motion_data
,
motion_cov
);
CaptureOdom2DPtr
cap_motion
=
std
::
make_shared
<
CaptureOdom2D
>
(
t
,
sensor_odo
,
motion_data
,
motion_cov
);
sensor_odo
->
process
(
cap_motion
);
// KF2
sensor_odo
->
process
(
cap_motion
);
// KF2
: (1,0,0)
// observe lmks
// observe lmks
ids
.
resize
(
2
);
ranges
.
resize
(
2
);
bearings
.
resize
(
2
);
ids
.
resize
(
2
);
ranges
.
resize
(
2
);
bearings
.
resize
(
2
);
...
@@ -190,14 +192,14 @@ int main()
...
@@ -190,14 +192,14 @@ int main()
ranges
<<
sqrt
(
2.0
),
1.0
;
// see drawing
ranges
<<
sqrt
(
2.0
),
1.0
;
// see drawing
bearings
<<
3
*
M_PI
/
4
,
M_PI
/
2
;
bearings
<<
3
*
M_PI
/
4
,
M_PI
/
2
;
cap_rb
=
std
::
make_shared
<
CaptureRangeBearing
>
(
t
,
sensor_rb
,
ids
,
ranges
,
bearings
);
cap_rb
=
std
::
make_shared
<
CaptureRangeBearing
>
(
t
,
sensor_rb
,
ids
,
ranges
,
bearings
);
sensor_rb
->
process
(
cap_rb
);
sensor_rb
->
process
(
cap_rb
);
// L1 : (1,2), L2 : (2,2)
// STEP 3 --------------------------------------------------------------
// STEP 3 --------------------------------------------------------------
t
+=
1.0
;
t
+=
1.0
;
// t : 2.0
// motion
// motion
cap_motion
->
setTimeStamp
(
t
);
cap_motion
->
setTimeStamp
(
t
);
sensor_odo
->
process
(
cap_motion
);
// KF3
sensor_odo
->
process
(
cap_motion
);
// KF3
: (2,0,0)
// observe lmks
// observe lmks
ids
.
resize
(
2
);
ranges
.
resize
(
2
);
bearings
.
resize
(
2
);
ids
.
resize
(
2
);
ranges
.
resize
(
2
);
bearings
.
resize
(
2
);
...
@@ -205,9 +207,7 @@ int main()
...
@@ -205,9 +207,7 @@ int main()
ranges
<<
sqrt
(
2.0
),
1.0
;
// see drawing
ranges
<<
sqrt
(
2.0
),
1.0
;
// see drawing
bearings
<<
3
*
M_PI
/
4
,
M_PI
/
2
;
bearings
<<
3
*
M_PI
/
4
,
M_PI
/
2
;
cap_rb
=
std
::
make_shared
<
CaptureRangeBearing
>
(
t
,
sensor_rb
,
ids
,
ranges
,
bearings
);
cap_rb
=
std
::
make_shared
<
CaptureRangeBearing
>
(
t
,
sensor_rb
,
ids
,
ranges
,
bearings
);
sensor_rb
->
process
(
cap_rb
);
sensor_rb
->
process
(
cap_rb
);
// L1 : (1,2), L2 : (2,2), L3 : (3,2)
problem
->
print
(
4
,
1
,
1
,
1
);
// SOLVE ================================================================
// SOLVE ================================================================
...
@@ -216,25 +216,30 @@ int main()
...
@@ -216,25 +216,30 @@ int main()
WOLF_TRACE
(
"======== SOLVE PROBLEM WITH EXACT PRIORS ======="
)
WOLF_TRACE
(
"======== SOLVE PROBLEM WITH EXACT PRIORS ======="
)
std
::
string
report
=
ceres
->
solve
(
wolf
::
SolverManager
::
ReportVerbosity
::
FULL
);
std
::
string
report
=
ceres
->
solve
(
wolf
::
SolverManager
::
ReportVerbosity
::
FULL
);
WOLF_TRACE
(
report
);
// should show a very low iteration number (possibly 1)
WOLF_TRACE
(
report
);
// should show a very low iteration number (possibly 1)
problem
->
print
(
4
,
1
,
1
,
1
);
problem
->
print
(
4
,
0
,
1
,
1
);
// PERTURB initial guess
// PERTURB initial guess
WOLF_TRACE
(
"======== PERTURB PROBLEM PRIORS ======="
)
WOLF_TRACE
(
"======== PERTURB PROBLEM PRIORS ======="
)
for
(
auto
sen
:
problem
->
getHardwarePtr
()
->
getSensorList
())
for
(
auto
sen
:
problem
->
getHardwarePtr
()
->
getSensorList
())
for
(
auto
sb
:
sen
->
getStateBlockVec
())
for
(
auto
sb
:
sen
->
getStateBlockVec
())
if
(
sb
&&
!
sb
->
isFixed
())
if
(
sb
&&
!
sb
->
isFixed
())
sb
->
setState
(
VectorXs
::
Random
(
sb
->
getSize
())
*
0.5
);
// We perturb A LOT !
sb
->
setState
(
sb
->
getState
()
+
VectorXs
::
Random
(
sb
->
getSize
())
*
0.5
);
// We perturb A LOT !
for
(
auto
kf
:
problem
->
getTrajectoryPtr
()
->
getFrameList
())
for
(
auto
kf
:
problem
->
getTrajectoryPtr
()
->
getFrameList
())
kf
->
setState
(
Vector3s
::
Random
()
*
0.5
);
// We perturb A LOT !
if
(
kf
->
isKey
())
for
(
auto
sb
:
kf
->
getStateBlockVec
())
if
(
sb
&&
!
sb
->
isFixed
())
sb
->
setState
(
sb
->
getState
()
+
VectorXs
::
Random
(
sb
->
getSize
())
*
0.5
);
// We perturb A LOT !
for
(
auto
lmk
:
problem
->
getMapPtr
()
->
getLandmarkList
())
for
(
auto
lmk
:
problem
->
getMapPtr
()
->
getLandmarkList
())
lmk
->
getPPtr
()
->
setState
(
Vector2s
::
Random
());
// We perturb A LOT !
for
(
auto
sb
:
lmk
->
getStateBlockVec
())
problem
->
print
(
4
,
1
,
1
,
1
);
if
(
sb
&&
!
sb
->
isFixed
())
sb
->
setState
(
sb
->
getState
()
+
VectorXs
::
Random
(
sb
->
getSize
())
*
0.5
);
// We perturb A LOT !
problem
->
print
(
2
,
0
,
1
,
1
);
// SOLVE again
// SOLVE again
WOLF_TRACE
(
"======== SOLVE PROBLEM WITH PERTURBED PRIORS ======="
)
WOLF_TRACE
(
"======== SOLVE PROBLEM WITH PERTURBED PRIORS ======="
)
report
=
ceres
->
solve
(
wolf
::
SolverManager
::
ReportVerbosity
::
FULL
);
report
=
ceres
->
solve
(
wolf
::
SolverManager
::
ReportVerbosity
::
FULL
);
WOLF_TRACE
(
report
);
// should show a very high iteration number (more than 10, or than 100!)
WOLF_TRACE
(
report
);
// should show a very high iteration number (more than 10, or than 100!)
problem
->
print
(
4
,
1
,
1
,
1
);
problem
->
print
(
1
,
1
,
1
,
1
);
// GET COVARIANCES of all states
// GET COVARIANCES of all states
WOLF_TRACE
(
"======== COVARIANCES OF SOLVED PROBLEM ======="
)
WOLF_TRACE
(
"======== COVARIANCES OF SOLVED PROBLEM ======="
)
...
...
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