-
Notifications
You must be signed in to change notification settings - Fork 0
/
local_coordinate_system.cpp
28 lines (22 loc) · 1.32 KB
/
local_coordinate_system.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
#include "local_coordinate_system.h"
#include "Eigen/Geometry"
LocalCoordinateSystem::LocalCoordinateSystem(const GeodeticPosition& origin)
: local_{origin.latitude, origin.longitude, origin.altitude}
{}
Sophus::SE3d LocalCoordinateSystem::toLocalPose(const GeodeticPosition& position_geodetic_body,
const Sophus::SO3d& orientation_ned_body) const
{
// Use a std::vector as storage for a row-major Eigen::Matrix3d.
std::vector<double> matrix_data(9);
Eigen::Map<Eigen::Matrix<double, 3, 3, Eigen::RowMajor>> R_local_ned(matrix_data.data());
Eigen::Vector3d position_enu_body;
// Transform geodetic coordinates to local (ENU-based) cartesian coordinate system.
local_.Forward(position_geodetic_body.latitude, position_geodetic_body.longitude, position_geodetic_body.altitude,
position_enu_body.x(), position_enu_body.y(), position_enu_body.z(),
matrix_data);
// Compute attitude in local (NED-based) coordinate system.
const Sophus::SO3d orientation_local_body{Sophus::SO3d{R_local_ned} * orientation_ned_body};
// Compute position in local (NED-based) coordinate system.
const Eigen::Vector3d position_local_body{position_enu_body.y(), position_enu_body.x(), -position_enu_body.z()};
return {orientation_local_body, position_local_body};
}