目录
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;
