Skip to content
GitLab
Explore
Sign in
Primary navigation
Search or go to…
Project
gnss_utils
Manage
Activity
Members
Labels
Plan
Issues
Issue boards
Milestones
Wiki
Code
Merge requests
Repository
Branches
Commits
Tags
Repository graph
Compare revisions
Snippets
Build
Pipelines
Jobs
Pipeline schedules
Artifacts
Deploy
Releases
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
labrobotica
algorithms
gnss_utils
Commits
8309b1ff
Commit
8309b1ff
authored
2 years ago
by
Joan Vallvé Navarro
Browse files
Options
Downloads
Patches
Plain Diff
rads and deg in latlonalt transformations
parent
cde27071
No related branches found
No related tags found
1 merge request
!27
Devel
Pipeline
#14191
passed
2 years ago
Changes
3
Pipelines
1
Hide whitespace changes
Inline
Side-by-side
Showing
3 changed files
include/gnss_utils/utils/transformations.h
+2
-2
2 additions, 2 deletions
include/gnss_utils/utils/transformations.h
src/utils/transformations.cpp
+21
-7
21 additions, 7 deletions
src/utils/transformations.cpp
test/gtest_transformations.cpp
+82
-6
82 additions, 6 deletions
test/gtest_transformations.cpp
with
105 additions
and
15 deletions
include/gnss_utils/utils/transformations.h
+
2
−
2
View file @
8309b1ff
...
...
@@ -36,8 +36,8 @@
namespace
GnssUtils
{
Eigen
::
Vector3d
ecefToLatLonAlt
(
const
Eigen
::
Vector3d
&
_ecef
);
Eigen
::
Vector3d
latLonAltToEcef
(
const
Eigen
::
Vector3d
&
_latlon
);
Eigen
::
Vector3d
ecefToLatLonAlt
(
const
Eigen
::
Vector3d
&
_ecef
,
bool
in_rads
=
true
);
Eigen
::
Vector3d
latLonAltToEcef
(
const
Eigen
::
Vector3d
&
_latlon
,
bool
in_rads
=
true
);
Eigen
::
Matrix3d
ecefToEnuCov
(
const
Eigen
::
Vector3d
&
_latlon
,
const
Eigen
::
Matrix3d
&
_cov_ecef
);
Eigen
::
Matrix3d
enuToEcefCov
(
const
Eigen
::
Vector3d
&
_latlon
,
const
Eigen
::
Matrix3d
&
_cov_enu
);
...
...
This diff is collapsed.
Click to expand it.
src/utils/transformations.cpp
+
21
−
7
View file @
8309b1ff
...
...
@@ -26,18 +26,32 @@ using namespace GnssUtils;
namespace
GnssUtils
{
Eigen
::
Vector3d
ecefToLatLonAlt
(
const
Eigen
::
Vector3d
&
_ecef
)
Eigen
::
Vector3d
ecefToLatLonAlt
(
const
Eigen
::
Vector3d
&
_ecef
,
bool
in_rads
)
{
Eigen
::
Vector3d
pos
;
ecef2pos
(
_ecef
.
data
(),
pos
.
data
());
return
pos
;
Eigen
::
Vector3d
latlonalt
;
ecef2pos
(
_ecef
.
data
(),
latlonalt
.
data
());
if
(
not
in_rads
)
{
latlonalt
(
0
)
=
RAD2DEG
*
latlonalt
(
0
);
latlonalt
(
1
)
=
RAD2DEG
*
latlonalt
(
1
);
}
return
latlonalt
;
}
Eigen
::
Vector3d
latLonAltToEcef
(
const
Eigen
::
Vector3d
&
_latlon
)
Eigen
::
Vector3d
latLonAltToEcef
(
const
Eigen
::
Vector3d
&
_latlon
,
bool
in_rads
)
{
Eigen
::
Vector3d
ecef
;
pos2ecef
(
_latlon
.
data
(),
ecef
.
data
());
if
(
not
in_rads
)
{
Eigen
::
Vector3d
latlon_rads
=
_latlon
;
latlon_rads
(
0
)
=
DEG2RAD
*
_latlon
(
0
);
latlon_rads
(
1
)
=
DEG2RAD
*
_latlon
(
1
);
latlon_rads
(
2
)
=
_latlon
(
2
);
pos2ecef
(
latlon_rads
.
data
(),
ecef
.
data
());
}
else
pos2ecef
(
_latlon
.
data
(),
ecef
.
data
());
return
ecef
;
}
...
...
This diff is collapsed.
Click to expand it.
test/gtest_transformations.cpp
+
82
−
6
View file @
8309b1ff
...
...
@@ -31,11 +31,11 @@ static double kSecondEccentricitySquared = 6.73949674228 * 0.001;
using
namespace
GnssUtils
;
Eigen
::
Vector3d
computeRandomReceiverLatLonAlt
()
Eigen
::
Vector3d
computeRandomReceiverLatLonAlt
(
bool
in_rads
=
true
)
{
Eigen
::
Vector3d
receiver_LLA
=
Eigen
::
Vector3d
::
Random
();
// in [-1, 1]
receiver_LLA
(
0
)
*=
M_PI
/
2
;
// in [-PI/2, PI/2]
receiver_LLA
(
1
)
*=
M_PI
;
// in [-PI, PI]
receiver_LLA
(
0
)
*=
(
in_rads
?
M_PI
/
2
:
90.0
)
;
// in [-PI/2, PI/2]
receiver_LLA
(
1
)
*=
(
in_rads
?
M_PI
:
180
)
;
// in [-PI, PI]
receiver_LLA
(
2
)
+=
1
;
// ([0, 2])
receiver_LLA
(
2
)
*=
5e2
;
// in [0, 1e3]
...
...
@@ -71,7 +71,7 @@ void computeRandomVisibleSatellite(const Eigen::Vector3d& receiver_latlonalt,
}
TEST
(
TransformationsTest
,
ecefToLatLonAlt
)
TEST
(
TransformationsTest
,
ecefToLatLonAlt
_rad
)
{
Eigen
::
Vector3d
ecef
,
latlonalt
;
...
...
@@ -97,7 +97,33 @@ TEST(TransformationsTest, ecefToLatLonAlt)
ASSERT_NEAR
(
latlonalt
(
1
),
0.0
,
1e-6
);
}
TEST
(
TransformationsTest
,
latLonAltToEcef
)
TEST
(
TransformationsTest
,
ecefToLatLonAlt_deg
)
{
Eigen
::
Vector3d
ecef
,
latlonalt
;
// aligned with ECEF X axis
ecef
<<
1e3
,
0
,
0
;
latlonalt
=
ecefToLatLonAlt
(
ecef
,
false
);
ASSERT_NEAR
(
latlonalt
(
0
),
0.0
,
1e-6
);
ASSERT_NEAR
(
latlonalt
(
1
),
0.0
,
1e-6
);
// aligned with ECEF Y axis
ecef
<<
0
,
1e3
,
0
;
latlonalt
=
ecefToLatLonAlt
(
ecef
,
false
);
ASSERT_NEAR
(
latlonalt
(
0
),
0.0
,
1e-6
);
ASSERT_NEAR
(
latlonalt
(
1
),
90
,
1e-6
);
// aligned with ECEF Z axis
ecef
<<
0
,
0
,
1e3
;
latlonalt
=
ecefToLatLonAlt
(
ecef
,
false
);
ASSERT_NEAR
(
latlonalt
(
0
),
90
,
1e-6
);
ASSERT_NEAR
(
latlonalt
(
1
),
0.0
,
1e-6
);
}
TEST
(
TransformationsTest
,
latLonAltToEcef_rad
)
{
Eigen
::
Vector3d
ecef
,
latlonalt
;
...
...
@@ -123,7 +149,33 @@ TEST(TransformationsTest, latLonAltToEcef)
ASSERT_NEAR
(
ecef
(
1
),
0.0
,
1e-6
);
}
TEST
(
TransformationsTest
,
latLonAltToEcef_ecefToLatLonAlt
)
TEST
(
TransformationsTest
,
latLonAltToEcef_deg
)
{
Eigen
::
Vector3d
ecef
,
latlonalt
;
// aligned with ECEF X axis
latlonalt
<<
0
,
0
,
0
;
ecef
=
latLonAltToEcef
(
latlonalt
,
false
);
ASSERT_NEAR
(
ecef
(
1
),
0.0
,
1e-6
);
ASSERT_NEAR
(
ecef
(
2
),
0.0
,
1e-6
);
// aligned with ECEF Y axis
latlonalt
<<
0
,
90
,
0
;
ecef
=
latLonAltToEcef
(
latlonalt
,
false
);
ASSERT_NEAR
(
ecef
(
0
),
0.0
,
1e-6
);
ASSERT_NEAR
(
ecef
(
2
),
0.0
,
1e-6
);
// aligned with ECEF Z axis
latlonalt
<<
90
,
0
,
0
;
ecef
=
latLonAltToEcef
(
latlonalt
,
false
);
ASSERT_NEAR
(
ecef
(
0
),
0.0
,
1e-6
);
ASSERT_NEAR
(
ecef
(
1
),
0.0
,
1e-6
);
}
TEST
(
TransformationsTest
,
latLonAltToEcef_ecefToLatLonAlt_rad
)
{
Eigen
::
Vector3d
ecef0
,
ecef1
,
latlonalt0
,
latlonalt1
;
...
...
@@ -147,6 +199,30 @@ TEST(TransformationsTest, latLonAltToEcef_ecefToLatLonAlt)
}
}
TEST
(
TransformationsTest
,
latLonAltToEcef_ecefToLatLonAlt_deg
)
{
Eigen
::
Vector3d
ecef0
,
ecef1
,
latlonalt0
,
latlonalt1
;
// ecef -> latlon -> ecef
for
(
auto
i
=
0
;
i
<
100
;
i
++
)
{
ecef0
=
1e3
*
Eigen
::
Vector3d
::
Random
();
latlonalt0
=
ecefToLatLonAlt
(
ecef0
,
false
);
ecef1
=
latLonAltToEcef
(
latlonalt0
,
false
);
ASSERT_MATRIX_APPROX
(
ecef0
,
ecef1
,
1e-6
);
}
// latlon -> ecef -> latlon
for
(
auto
i
=
0
;
i
<
100
;
i
++
)
{
latlonalt0
=
computeRandomReceiverLatLonAlt
(
false
);
ecef0
=
latLonAltToEcef
(
latlonalt0
,
false
);
latlonalt1
=
ecefToLatLonAlt
(
ecef0
,
false
);
ASSERT_MATRIX_APPROX
(
latlonalt0
,
latlonalt1
,
1e-6
);
}
}
TEST
(
TransformationsTest
,
computeEnuEcef
)
{
Eigen
::
Vector3d
t_ECEF_ENU
,
t_ENU_latlonalt
,
latlonalt1
;
...
...
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