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
454e0301
Commit
454e0301
authored
6 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
ceres bug investigation
parent
bac5a67e
No related branches found
No related tags found
1 merge request
!234
WIP: Gnss
Pipeline
#2531
failed
6 years ago
Changes
3
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
test/CMakeLists.txt
+7
-0
7 additions, 0 deletions
test/CMakeLists.txt
test/gtest_constraint_gnss_fix_2D_1.cpp
+155
-0
155 additions, 0 deletions
test/gtest_constraint_gnss_fix_2D_1.cpp
test/gtest_constraint_gnss_fix_2D_2.cpp
+155
-0
155 additions, 0 deletions
test/gtest_constraint_gnss_fix_2D_2.cpp
with
317 additions
and
0 deletions
test/CMakeLists.txt
+
7
−
0
View file @
454e0301
...
...
@@ -148,6 +148,13 @@ ENDIF(vision_utils_FOUND)
wolf_add_gtest
(
gtest_constraint_gnss_fix_2D gtest_constraint_gnss_fix_2D.cpp
)
target_link_libraries
(
gtest_constraint_gnss_fix_2D
${
PROJECT_NAME
}
)
# ConstraintGnssFix2D test
wolf_add_gtest
(
gtest_constraint_gnss_fix_2D_1 gtest_constraint_gnss_fix_2D_1.cpp
)
target_link_libraries
(
gtest_constraint_gnss_fix_2D_1
${
PROJECT_NAME
}
)
# ConstraintGnssFix2D test
wolf_add_gtest
(
gtest_constraint_gnss_fix_2D_2 gtest_constraint_gnss_fix_2D_2.cpp
)
target_link_libraries
(
gtest_constraint_gnss_fix_2D_2
${
PROJECT_NAME
}
)
# ConstraintGnssFix2D test
wolf_add_gtest
(
gtest_constraint_gnss_fix_2D_simple gtest_constraint_gnss_fix_2D_simple.cpp
)
target_link_libraries
(
gtest_constraint_gnss_fix_2D_simple
${
PROJECT_NAME
}
)
...
...
This diff is collapsed.
Click to expand it.
test/gtest_constraint_gnss_fix_2D_1.cpp
0 → 100644
+
155
−
0
View file @
454e0301
/**
* \file gtest_constraint_gnss_fix_2D.cpp
*
* Created on: Aug 1, 2018
* \author: jvallve
*/
#include
"base/constraint/constraint_gnss_fix_2D.h"
#include
"utils_gtest.h"
#include
"base/capture/capture_motion.h"
#include
"base/problem.h"
#include
"base/sensor/sensor_gnss.h"
#include
"base/processor/processor_gnss_fix.h"
#include
"base/ceres_wrapper/ceres_manager.h"
using
namespace
Eigen
;
using
namespace
wolf
;
using
std
::
cout
;
using
std
::
endl
;
class
ConstraintGnssFix2DTest
:
public
testing
::
Test
{
public:
// groundtruth transformations
Vector3s
t_ecef_enu
;
Matrix3s
R_ecef_enu
,
R_enu_map
,
R_map_base
;
Vector3s
t_base_antena
,
t_ecef_antena
;
Vector1s
o_enu_map
;
Vector3s
t_enu_map
;
Vector3s
t_map_base
;
Vector1s
o_map_base
;
// WOLF
ProblemPtr
problem_ptr
;
CeresManagerPtr
ceres_mgr_ptr
;
SensorGnssPtr
gnss_sensor_ptr
;
ConstraintGnssFix2DTest
()
{
o_enu_map
<<
2.6
;
t_enu_map
<<
12
,
-
34
,
4
;
t_map_base
<<
32
,
-
34
,
0
;
// z=0
o_map_base
<<
-
0.4
;
t_base_antena
<<
3
,
-
2
,
8
;
// Antena extrinsics
t_ecef_enu
<<
100
,
30
,
50
;
R_ecef_enu
=
(
AngleAxis
<
Scalar
>
(
1.3
,
Vector3s
::
UnitX
())
*
AngleAxis
<
Scalar
>
(
-
2.2
,
Vector3s
::
UnitY
())
*
AngleAxis
<
Scalar
>
(
-
1.8
,
Vector3s
::
UnitZ
())).
toRotationMatrix
();
R_enu_map
=
AngleAxis
<
Scalar
>
(
o_enu_map
(
0
),
Vector3s
::
UnitZ
()).
toRotationMatrix
();
R_map_base
=
Matrix3s
::
Identity
();
R_map_base
.
topLeftCorner
(
2
,
2
)
=
Rotation2D
<
Scalar
>
(
o_map_base
(
0
)).
matrix
();
t_ecef_antena
=
R_ecef_enu
*
R_enu_map
*
(
R_map_base
*
t_base_antena
+
t_map_base
)
+
t_ecef_enu
;
// ----------------------------------------------------
// Problem and solver
problem_ptr
=
Problem
::
create
(
"PO 2D"
);
ceres_mgr_ptr
=
std
::
make_shared
<
CeresManager
>
(
problem_ptr
);
ceres_mgr_ptr
->
getSolverOptions
().
max_num_iterations
=
100
;
// gnss sensor & processor
gnss_sensor_ptr
=
std
::
static_pointer_cast
<
SensorGnss
>
(
problem_ptr
->
installSensor
(
"GNSS"
,
"gnss"
,
t_base_antena
,
std
::
make_shared
<
IntrinsicsBase
>
()));
gnss_sensor_ptr
->
getEnuMapTranslationStatePtr
()
->
setState
(
t_enu_map
);
gnss_sensor_ptr
->
getEnuMapYawStatePtr
()
->
setState
(
o_enu_map
);
gnss_sensor_ptr
->
getEnuMapRollStatePtr
()
->
fix
();
gnss_sensor_ptr
->
getEnuMapPitchStatePtr
()
->
fix
();
gnss_sensor_ptr
->
setEnuEcef
(
R_ecef_enu
.
transpose
(),
R_ecef_enu
.
transpose
()
*
(
-
t_ecef_enu
));
std
::
cout
<<
"gnss sensor installed"
<<
std
::
endl
;
ProcessorParamsGnssFixPtr
gnss_params_ptr
=
std
::
make_shared
<
ProcessorParamsGnssFix
>
();
gnss_params_ptr
->
time_tolerance
=
-
1.0
;
gnss_params_ptr
->
voting_active
=
true
;
problem_ptr
->
installProcessor
(
"GNSS FIX"
,
"gnss fix"
,
gnss_sensor_ptr
,
gnss_params_ptr
);
std
::
cout
<<
"gnss processor installed"
<<
std
::
endl
;
// set prior (FIXED)
Eigen
::
Vector3s
frame_pose
;
frame_pose
<<
t_map_base
.
head
(
2
),
o_map_base
;
FrameBasePtr
prior_frame_ptr
=
problem_ptr
->
setPrior
(
frame_pose
,
Matrix3s
::
Identity
(),
TimeStamp
(
0
),
Scalar
(
0.1
));
prior_frame_ptr
->
fix
();
};
virtual
void
SetUp
()
{
// reset grountruth parameters
R_enu_map
=
AngleAxis
<
Scalar
>
(
o_enu_map
(
0
),
Vector3s
::
UnitZ
()).
toRotationMatrix
();
R_map_base
=
Matrix3s
::
Identity
();
R_map_base
.
topLeftCorner
(
2
,
2
)
=
Rotation2D
<
Scalar
>
(
o_map_base
(
0
)).
matrix
();
t_ecef_antena
=
R_ecef_enu
*
(
R_enu_map
*
(
R_map_base
*
t_base_antena
+
t_map_base
)
+
t_enu_map
)
+
t_ecef_enu
;
// Reset antena extrinsics
gnss_sensor_ptr
->
getPPtr
()
->
setState
(
t_base_antena
);
// Reset ENU_ECEF
gnss_sensor_ptr
->
setEnuEcef
(
R_ecef_enu
.
transpose
(),
R_ecef_enu
.
transpose
()
*
(
-
t_ecef_enu
));
}
};
////////////////////////////////////////////////////////
TEST_F
(
ConstraintGnssFix2DTest
,
check_tree
)
{
ASSERT_TRUE
(
problem_ptr
->
check
(
0
));
}
/*
* Test with one GNSS fix capture.
*
* Estimating: MAP_BASE position.
* Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
*/
TEST_F
(
ConstraintGnssFix2DTest
,
gnss_1_map_base_position
)
{
// Create GNSS Fix capture
CaptureGnssFixPtr
cap_gnss_ptr
=
std
::
make_shared
<
CaptureGnssFix
>
(
TimeStamp
(
0
),
gnss_sensor_ptr
,
t_ecef_antena
,
1e-3
*
Matrix3s
::
Identity
());
gnss_sensor_ptr
->
process
(
cap_gnss_ptr
);
// fixing things
gnss_sensor_ptr
->
getEnuMapTranslationStatePtr
()
->
fix
();
// enu-map yaw translation
gnss_sensor_ptr
->
getEnuMapYawStatePtr
()
->
fix
();
// enu-map yaw orientation
cap_gnss_ptr
->
getFramePtr
()
->
getOPtr
()
->
setState
(
o_map_base
);
// frame orientation
cap_gnss_ptr
->
getFramePtr
()
->
getOPtr
()
->
fix
();
// distort frm position
Vector3s
frm_dist
=
cap_gnss_ptr
->
getFramePtr
()
->
getState
();
frm_dist
(
0
)
+=
0.18
;
frm_dist
(
1
)
+=
-
0.58
;
cap_gnss_ptr
->
getFramePtr
()
->
setState
(
frm_dist
);
// solve for frm
std
::
string
report
=
ceres_mgr_ptr
->
solve
(
SolverManager
::
ReportVerbosity
::
FULL
);
std
::
cout
<<
report
<<
std
::
endl
;
ASSERT_EQ
(
ceres_mgr_ptr
->
getSummary
().
num_parameter_blocks_reduced
,
1
);
ASSERT_EQ
(
ceres_mgr_ptr
->
getSummary
().
num_parameters_reduced
,
2
);
ASSERT_EQ
(
ceres_mgr_ptr
->
getSummary
().
num_residual_blocks_reduced
,
1
);
ASSERT_EQ
(
ceres_mgr_ptr
->
getSummary
().
num_residuals_reduced
,
3
);
ASSERT_TRUE
(
cap_gnss_ptr
->
getFramePtr
()
->
isKey
());
ASSERT_MATRIX_APPROX
(
cap_gnss_ptr
->
getFramePtr
()
->
getState
().
head
(
2
),
t_map_base
.
head
(
2
),
1e-6
);
cap_gnss_ptr
->
getFramePtr
()
->
remove
();
}
int
main
(
int
argc
,
char
**
argv
)
{
testing
::
InitGoogleTest
(
&
argc
,
argv
);
return
RUN_ALL_TESTS
();
}
This diff is collapsed.
Click to expand it.
test/gtest_constraint_gnss_fix_2D_2.cpp
0 → 100644
+
155
−
0
View file @
454e0301
/**
* \file gtest_constraint_gnss_fix_2D.cpp
*
* Created on: Aug 1, 2018
* \author: jvallve
*/
#include
"base/constraint/constraint_gnss_fix_2D.h"
#include
"utils_gtest.h"
#include
"base/capture/capture_motion.h"
#include
"base/problem.h"
#include
"base/sensor/sensor_gnss.h"
#include
"base/processor/processor_gnss_fix.h"
#include
"base/ceres_wrapper/ceres_manager.h"
using
namespace
Eigen
;
using
namespace
wolf
;
using
std
::
cout
;
using
std
::
endl
;
class
ConstraintGnssFix2DTest
:
public
testing
::
Test
{
public:
// groundtruth transformations
Vector3s
t_ecef_enu
;
Matrix3s
R_ecef_enu
,
R_enu_map
,
R_map_base
;
Vector3s
t_base_antena
,
t_ecef_antena
;
Vector1s
o_enu_map
;
Vector3s
t_enu_map
;
Vector3s
t_map_base
;
Vector1s
o_map_base
;
// WOLF
ProblemPtr
problem_ptr
;
CeresManagerPtr
ceres_mgr_ptr
;
SensorGnssPtr
gnss_sensor_ptr
;
ConstraintGnssFix2DTest
()
{
o_enu_map
<<
2.6
;
t_enu_map
<<
12
,
-
34
,
4
;
t_map_base
<<
32
,
-
34
,
0
;
// z=0
o_map_base
<<
-
0.4
;
t_base_antena
<<
3
,
-
2
,
8
;
// Antena extrinsics
t_ecef_enu
<<
100
,
30
,
50
;
R_ecef_enu
=
(
AngleAxis
<
Scalar
>
(
1.3
,
Vector3s
::
UnitX
())
*
AngleAxis
<
Scalar
>
(
-
2.2
,
Vector3s
::
UnitY
())
*
AngleAxis
<
Scalar
>
(
-
1.8
,
Vector3s
::
UnitZ
())).
toRotationMatrix
();
R_enu_map
=
AngleAxis
<
Scalar
>
(
o_enu_map
(
0
),
Vector3s
::
UnitZ
()).
toRotationMatrix
();
R_map_base
=
Matrix3s
::
Identity
();
R_map_base
.
topLeftCorner
(
2
,
2
)
=
Rotation2D
<
Scalar
>
(
o_map_base
(
0
)).
matrix
();
t_ecef_antena
=
R_ecef_enu
*
R_enu_map
*
(
R_map_base
*
t_base_antena
+
t_map_base
)
+
t_ecef_enu
;
// ----------------------------------------------------
// Problem and solver
problem_ptr
=
Problem
::
create
(
"PO 2D"
);
ceres_mgr_ptr
=
std
::
make_shared
<
CeresManager
>
(
problem_ptr
);
ceres_mgr_ptr
->
getSolverOptions
().
max_num_iterations
=
100
;
// gnss sensor & processor
gnss_sensor_ptr
=
std
::
static_pointer_cast
<
SensorGnss
>
(
problem_ptr
->
installSensor
(
"GNSS"
,
"gnss"
,
t_base_antena
,
std
::
make_shared
<
IntrinsicsBase
>
()));
gnss_sensor_ptr
->
getEnuMapTranslationStatePtr
()
->
setState
(
t_enu_map
);
gnss_sensor_ptr
->
getEnuMapYawStatePtr
()
->
setState
(
o_enu_map
);
gnss_sensor_ptr
->
getEnuMapRollStatePtr
()
->
fix
();
gnss_sensor_ptr
->
getEnuMapPitchStatePtr
()
->
fix
();
gnss_sensor_ptr
->
setEnuEcef
(
R_ecef_enu
.
transpose
(),
R_ecef_enu
.
transpose
()
*
(
-
t_ecef_enu
));
std
::
cout
<<
"gnss sensor installed"
<<
std
::
endl
;
ProcessorParamsGnssFixPtr
gnss_params_ptr
=
std
::
make_shared
<
ProcessorParamsGnssFix
>
();
gnss_params_ptr
->
time_tolerance
=
-
1.0
;
gnss_params_ptr
->
voting_active
=
true
;
problem_ptr
->
installProcessor
(
"GNSS FIX"
,
"gnss fix"
,
gnss_sensor_ptr
,
gnss_params_ptr
);
std
::
cout
<<
"gnss processor installed"
<<
std
::
endl
;
// set prior (FIXED)
Eigen
::
Vector3s
frame_pose
;
frame_pose
<<
t_map_base
.
head
(
2
),
o_map_base
;
FrameBasePtr
prior_frame_ptr
=
problem_ptr
->
setPrior
(
frame_pose
,
Matrix3s
::
Identity
(),
TimeStamp
(
0
),
Scalar
(
0.1
));
prior_frame_ptr
->
fix
();
};
virtual
void
SetUp
()
{
// reset grountruth parameters
R_enu_map
=
AngleAxis
<
Scalar
>
(
o_enu_map
(
0
),
Vector3s
::
UnitZ
()).
toRotationMatrix
();
R_map_base
=
Matrix3s
::
Identity
();
R_map_base
.
topLeftCorner
(
2
,
2
)
=
Rotation2D
<
Scalar
>
(
o_map_base
(
0
)).
matrix
();
t_ecef_antena
=
R_ecef_enu
*
(
R_enu_map
*
(
R_map_base
*
t_base_antena
+
t_map_base
)
+
t_enu_map
)
+
t_ecef_enu
;
// Reset antena extrinsics
gnss_sensor_ptr
->
getPPtr
()
->
setState
(
t_base_antena
);
// Reset ENU_ECEF
gnss_sensor_ptr
->
setEnuEcef
(
R_ecef_enu
.
transpose
(),
R_ecef_enu
.
transpose
()
*
(
-
t_ecef_enu
));
}
};
////////////////////////////////////////////////////////
TEST_F
(
ConstraintGnssFix2DTest
,
check_tree
)
{
ASSERT_TRUE
(
problem_ptr
->
check
(
0
));
}
/*
* Test with one GNSS fix capture.
*
* Estimating: MAP_BASE position.
* Fixed: ENU_MAP, MAP_BASE orientation, BASE_ANTENA.
*/
TEST_F
(
ConstraintGnssFix2DTest
,
gnss_1_map_base_position
)
{
// Create GNSS Fix capture
CaptureGnssFixPtr
cap_gnss_ptr
=
std
::
make_shared
<
CaptureGnssFix
>
(
TimeStamp
(
0
),
gnss_sensor_ptr
,
t_ecef_antena
,
1e-3
*
Matrix3s
::
Identity
());
gnss_sensor_ptr
->
process
(
cap_gnss_ptr
);
// fixing things
gnss_sensor_ptr
->
getEnuMapTranslationStatePtr
()
->
fix
();
// enu-map yaw translation
gnss_sensor_ptr
->
getEnuMapYawStatePtr
()
->
fix
();
// enu-map yaw orientation
cap_gnss_ptr
->
getFramePtr
()
->
getOPtr
()
->
setState
(
o_map_base
);
// frame orientation
cap_gnss_ptr
->
getFramePtr
()
->
getOPtr
()
->
fix
();
// distort frm position
Vector3s
frm_dist
=
cap_gnss_ptr
->
getFramePtr
()
->
getState
();
frm_dist
(
0
)
+=
0.18
;
frm_dist
(
1
)
+=
-
0.58
;
cap_gnss_ptr
->
getFramePtr
()
->
setState
(
frm_dist
);
// solve for frm
std
::
string
report
=
ceres_mgr_ptr
->
solve
(
SolverManager
::
ReportVerbosity
::
FULL
);
std
::
cout
<<
report
<<
std
::
endl
;
ASSERT_EQ
(
ceres_mgr_ptr
->
getSummary
().
num_parameter_blocks_reduced
,
1
);
ASSERT_EQ
(
ceres_mgr_ptr
->
getSummary
().
num_parameters_reduced
,
2
);
ASSERT_EQ
(
ceres_mgr_ptr
->
getSummary
().
num_residual_blocks_reduced
,
1
);
ASSERT_EQ
(
ceres_mgr_ptr
->
getSummary
().
num_residuals_reduced
,
3
);
ASSERT_TRUE
(
cap_gnss_ptr
->
getFramePtr
()
->
isKey
());
ASSERT_MATRIX_APPROX
(
cap_gnss_ptr
->
getFramePtr
()
->
getState
().
head
(
2
),
t_map_base
.
head
(
2
),
1e-6
);
cap_gnss_ptr
->
getFramePtr
()
->
remove
();
}
int
main
(
int
argc
,
char
**
argv
)
{
testing
::
InitGoogleTest
(
&
argc
,
argv
);
return
RUN_ALL_TESTS
();
}
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