#include "Coordinate_Transformation.h"
#include <Eigen/Dense>
namespace hdmap
{
Coordinate_Transformation::Coordinate_Transformation(/* args */) {}
Coordinate_Transformation::~Coordinate_Transformation() {}
PublishData::Point3D Coordinate_Transformation::LLH2XYZ(PublishData::PointLLH anchor_point, PublishData::PointLLH vehicle_global_coordinates)
{
PublishData::Point3D vehicle_map_coordinates;
anchor_point.lon = anchor_point.lon * M_PI / 180.0;
anchor_point.lat = anchor_point.lat * M_PI / 180.0;
anchor_point.height = anchor_point.height;
vehicle_global_coordinates.lon = vehicle_global_coordinates.lon * M_PI / 180.0;
vehicle_global_coordinates.lat = vehicle_global_coordinates.lat * M_PI / 180.0;
vehicle_global_coordinates.height = vehicle_global_coordinates.height;
double f = 1 / 298.257; // oblateness
double e = sqrt(2 * f - f * f); // eccentricity
double a = 6378137.; //
double N0 = a / sqrt(1 - e * e * sin(anchor_point.lat) * sin(anchor_point.lat));
double N1 = a / sqrt(1 - e * e * sin(vehicle_global_coordinates.lat) * sin(vehicle_global_coordinates.lat));
Eigen::MatrixXd R(3, 3); // calculate the coordinate of the traffic light in map coordination system
Eigen::MatrixXd M(3, 1);
Eigen::MatrixXd X(3, 1);
R(0, 0) = -sin(anchor_point.lon);
R(0, 1) = cos(anchor_point.lon);
R(0, 2) = 0;
R(1, 0) = -sin(anchor_point.lat) * cos(anchor_point.lon);
R(1, 1) = -sin(anchor_point.lat) * sin(anchor_point.lon);
R(1, 2) = cos(anchor_point.lat);
R(2, 0) = cos(anchor_point.lat) * cos(anchor_point.lon);
R(2, 1) = cos(anchor_point.lat) * sin(anchor_point.lon);
R(2, 2) = sin(anchor_point.lat);
M(0, 0) = (N1 + vehicle_global_coordinates.height) * cos(vehicle_global_coordinates.lat) * cos(vehicle_global_coordinates.lon) -
(N0 + anchor_point.height) * cos(anchor_point.lat) * cos(anchor_point.lon);
M(1, 0) = (N1 + vehicle_global_coordinates.height) * cos(vehicle_global_coordinates.lat) * sin(vehicle_global_coordinates.lon) -
(N0 + anchor_point.height) * cos(anchor_point.lat) * sin(anchor_point.lon);
M(2, 0) = (N1 * (1 - e * e) + vehicle_global_coordinates.height) * sin(vehicle_global_coordinates.lat) - (N0 * (1 - e * e) + anchor_point.height) * sin(anchor_point.lat);
X = R * M;
vehicle_map_coordinates.x = X(0, 0);
vehicle_map_coordinates.y = X(1, 0);
vehicle_map_coordinates.z = X(2, 0);
return vehicle_map_coordinates;
}
PublishData::PointLLH Coordinate_Transformation::XYZ2LLH(PublishData::PointLLH anchor, PublishData::Point3D point_enu)
{
PublishData::PointLLH point_llh;
anchor.lat = anchor.lat* M_PI/180.0 ;
anchor.lon = anchor.lon* M_PI/180.0;
double f = 1.0 / 298.257;
double e = sqrt(2 * f - f * f);
double a = 6378137.0;
double N1 = a / sqrt(1 - e * e * sin(anchor.lat) * sin(anchor.lat));
Eigen::MatrixXd X(3, 1);
Eigen::MatrixXd R(3, 3);
Eigen::MatrixXd M(3, 1);
R(0, 0) = -sin(anchor.lon);
R(0, 1) = cos(anchor.lon);
R(0, 2) = 0;
R(1, 0) = -sin(anchor.lat) * cos(anchor.lon);
R(1, 1) = -sin(anchor.lat) * sin(anchor.lon);
R(1, 2) = cos(anchor.lat);
R(2, 0) = cos(anchor.lat) * cos(anchor.lon);
R(2, 1) = cos(anchor.lat) * sin(anchor.lon);
R(2, 2) = sin(anchor.lat);
X(0, 0) = (N1 + anchor.height) * cos(anchor.lat) * cos(anchor.lon);
X(1, 0) = (N1 + anchor.height) * cos(anchor.lat) * sin(anchor.lon);
X(2, 0) = (N1 * (1 - e * e) + anchor.height) * sin(anchor.lat);
M(0, 0) = point_enu.x;
M(1, 0) = point_enu.y;
M(2, 0) = point_enu.z;
M = R.transpose() * M + X;
point_llh.lon = atan2(M(1, 0), M(0, 0));
double temp1, temp2, temp3;
temp1 = atan(M(2, 0) / sqrt(M(0, 0) * M(0, 0) + M(1, 0) * M(1, 0)));
double N = a / sqrt(1 - e * e * sin(temp1) * sin(temp1));
// temp2 = atan(tan(temp1) + N * e * e * sin(temp1) / sqrt(M(0, 0) * M(0, 0) + M(1, 0) * M(1, 0)));
temp2 = atan(M(2, 0) * (1 + e * e) / sqrt(M(0, 0) * M(0, 0) + M(1, 0) * M(1, 0)));
for (int i = 0; i < 100; i++)
{
N = a / sqrt(1 - e * e * sin(temp2) * sin(temp2));
temp3 = atan(tan(temp1) + N * e * e * sin(temp2) / sqrt(M(0, 0) * M(0, 0) + M(1, 0) * M(1, 0)));
if (abs(temp2 - temp3) < 1e-16)
{
// std::cout << "iter: " << i << std::endl;
break;
}
if (i == 100 && abs(temp2 - temp3) > 1e-14)
{
std::cout << "ERROR: OUT OF ITER!" << std::endl;
// std::cout << "iter: " << i << std::endl;
}
temp2 = temp3;
}
point_llh.lat = temp3 * 180.0 / M_PI;
point_llh.lon = point_llh.lon * 180.0 / M_PI;
point_llh.height = sqrt(M(0, 0) * M(0, 0) + M(1, 0) * M(1, 0)) / cos(temp3) - (a / sqrt(1 - e * e * sin(temp3) * sin(temp3)));
return point_llh;
}
} // namespace hdmap
