目录

1、前言

2、示例

3、代码解析

4、垂直于给定点的切平面变换

5、代码解析

---

1、前言

在地球表面进行刚体变换时候，要将具有经纬度、高程和偏北角的坐标信息转换为变换矩阵表达，首先需要了解坐标系之间的转换关系。

通常，我们会将经纬度坐标转换为地心坐标系（ECEF坐标系），然后再根据高程和偏北角进行变换。

2、示例

#include <iostream>
#include <Eigen/Dense>
#include <cmath>// 定义常量
constexpr double PI = 3.14159265358979323846;
constexpr double EARTH_RADIUS = 6378137.0; // 地球半径，单位：米// 经纬度转ECEF坐标
Eigen::Vector3d geodeticToECEF(double latitude, double longitude, double altitude) {double cosLat = cos(latitude * PI / 180);double sinLat = sin(latitude * PI / 180);double cosLon = cos(longitude * PI / 180);double sinLon = sin(longitude * PI / 180);double N = EARTH_RADIUS / sqrt(1 - pow(0.08181919, 2) * pow(sinLat, 2));double x = (N + altitude) * cosLat * cosLon;double y = (N + altitude) * cosLat * sinLon;double z = ((1 - pow(0.08181919, 2)) * N + altitude) * sinLat;return Eigen::Vector3d(x, y, z);
}int main() {// 假设经纬度坐标为纽约市的中心，高程为0，偏北角为30度double latitude = 40.7128; // 纬度，单位：度double longitude = -74.0060; // 经度，单位：度double altitude = 0; // 高程，单位：米double heading_angle = 30; // 偏北角，单位：度// 经纬度转换为ECEF坐标Eigen::Vector3d ecef = geodeticToECEF(latitude, longitude, altitude);// 构造变换矩阵Eigen::Affine3d transformation_matrix = Eigen::Affine3d::Identity();transformation_matrix.translation() = ecef;// 对Z轴进行旋转，旋转角度为偏北角transformation_matrix.rotate(Eigen::AngleAxisd(heading_angle * PI / 180, Eigen::Vector3d::UnitZ()));// 输出变换矩阵std::cout << "Transformation matrix:" << std::endl << transformation_matrix.matrix() << std::endl;return 0;
}

3、代码解析

• 经纬度转笛卡尔坐标

经纬度转笛卡尔坐标_经纬度转笛卡尔坐标系-CSDN博客

• 仿射变换

同样，拓展到三维，也有一样的规律。

拓展：

细心的小伙伴，肯定会注意到，此时计算出来的变换矩阵不是我们地球表面物体的变换矩阵，为什么的？因为物体都是在给定点的切平面上摆放的，so......

4、垂直于给定点的切平面变换

// 输入经纬度、高程和偏北角
Vector3d latLonAlt(37.7749, -122.4194, 10.0); // 纬度、经度、高程
double heading = 45.0 / RAD_TO_DEG; // 偏北角// 构造仿射变换矩阵
Affine3d transformMatrix = buildTransformMatrix(latLonAlt, heading);// 添加垂直切平面变换
Vector3d normalVector = transformMatrix.linear().col(2); // 取仿射变换矩阵的第三列作为法向量
Vector3d pointOnPlane = transformMatrix.translation(); // 平移部分即是平面上的一点Affine3d verticalPlaneTransform = Affine3d::Identity();    verticalPlaneTransform.translation() = pointOnPlane; // 平移至给定点verticalPlaneTransform.linear().col(0) = normalVector; // 设置 x 轴方向为法向量方向// 设置 y 轴方向为法向量与 x 轴叉乘的结果，即平面上的一个方向向量
verticalPlaneTransform.linear().col(1) = Vector3d::UnitX().cross(normalVector).normalized(); 
// z 轴方向为法向量方向，保证右手坐标系
verticalPlaneTransform.linear().col(2) = normalVector; // 打印变换矩阵
std::cout << "Transform Matrix:" << std::endl << transformMatrix.matrix() << std::endl;
std::cout << "Vertical Plane Transform Matrix:" << std::endl << verticalPlaneTransform.matrix() << std::endl;

5、代码解析

• 怎么定切平面的法向量

从仿射变换矩阵中提取法向量，您可以简单地提取矩阵的旋转部分的第三列（或者第三行，根据矩阵的定义）。在仿射变换中，旋转矩阵描述了变换的旋转部分，而第三列（或第三行）代表了变换后的 z 轴方向，因此它也是平面的法向量。

对于仿射变换矩阵来说，通常我们使用列向量表示基向量的变换。在三维仿射变换中，通常情况下，矩阵的前三列描述了变换后的基向量在原基向量上的坐标，其中第一列是 x 轴方向，第二列是 y 轴方向，第三列是 z 轴方向。

因此，对于一个仿射变换矩阵，第三列描述了变换后的 z 轴方向，这也就是平面的法向量。而平面的法向量描述了平面的法线方向，因此我们可以从仿射变换矩阵的第三列提取出平面的法向量。

// 定义一个仿射变换矩阵Affine3d transformMatrix = Affine3d::Identity();transformMatrix.translation() << 1.0, 2.0, 3.0;transformMatrix.linear() << 1.0, 0.0, 0.0,0.0, 1.0, 0.0,0.0, 0.0, 1.0;// 提取法向量
Vector3d normalVector = transformMatrix.linear().col(2);// 打印法向量
std::cout << "Normal Vector:" << std::endl << normalVector << std::endl;