修改成像网格平面计算bug
parent
6463cd4ccd
commit
7557d35968
|
|
@ -66,6 +66,9 @@ inline char* get_cur_time() {
|
|||
#define EARTHWE 0.000072292115
|
||||
#define PI 3.141592653589793238462643383279
|
||||
|
||||
#define WGS84_A 6378137.0 // 长半轴 (m)
|
||||
#define WGS84_F (1.0/298.257223563)
|
||||
#define WGS84_B (WGS84_A*(1-WGS84_F)) // 短半轴 (m)
|
||||
|
||||
|
||||
#define earthRoute 0.000072292115
|
||||
|
|
@ -140,7 +143,9 @@ struct DemBox {
|
|||
double max_lat;
|
||||
};
|
||||
|
||||
struct Vector3 { double x, y, z; };
|
||||
struct Vector3 {
|
||||
double x, y, z;
|
||||
};
|
||||
|
||||
|
||||
/*********************************************** FEKO仿真参数 ********************************************************************/
|
||||
|
|
|
|||
|
|
@ -1207,7 +1207,7 @@ void gdalImage::saveImage(std::shared_ptr<double> data, int start_row, int start
|
|||
}
|
||||
else {
|
||||
poDstDS = poDriver->Create(this->img_path.toUtf8().constData(), this->width, this->height,
|
||||
this->band_num, GDT_Float32, NULL); // 斤拷锟斤拷
|
||||
this->band_num, GDT_Float64, NULL); // 斤拷锟斤拷
|
||||
poDstDS->SetProjection(this->projection.toUtf8().constData());
|
||||
|
||||
double gt_ptr[6];
|
||||
|
|
@ -1690,7 +1690,7 @@ gdalImage CreategdalImageDouble(const QString& img_path, int height, int width,
|
|||
}
|
||||
|
||||
gdalImage CreategdalImage(const QString& img_path, int height, int width, int band_num,
|
||||
Eigen::MatrixXd gt, QString projection, bool need_gt, bool overwrite, bool isEnvi)
|
||||
Eigen::MatrixXd gt, QString projection,bool need_gt, bool overwrite, bool isEnvi, GDALDataType datetype)
|
||||
{
|
||||
if(exists_test(img_path.toUtf8().constData())) {
|
||||
if(overwrite) {
|
||||
|
|
@ -1713,7 +1713,7 @@ gdalImage CreategdalImage(const QString& img_path, int height, int width, int ba
|
|||
|
||||
|
||||
GDALDataset* poDstDS = poDriver->Create(img_path.toUtf8().constData(), width, height, band_num,
|
||||
GDT_Float32, NULL); // 锟斤拷锟斤拷锟斤拷
|
||||
datetype, NULL); // 锟斤拷锟斤拷锟斤拷
|
||||
if(need_gt) {
|
||||
if (!projection.isEmpty()) {
|
||||
poDstDS->SetProjection(projection.toUtf8().constData());
|
||||
|
|
|
|||
|
|
@ -240,7 +240,7 @@ public:
|
|||
|
||||
// 创建影像
|
||||
gdalImage BASECONSTVARIABLEAPI CreategdalImageDouble(const QString& img_path, int height, int width, int band_num, Eigen::MatrixXd gt, QString projection, bool need_gt = true, bool overwrite = false, bool isEnvi = false);
|
||||
gdalImage BASECONSTVARIABLEAPI CreategdalImage(const QString& img_path, int height, int width, int band_num, Eigen::MatrixXd gt, QString projection, bool need_gt = true, bool overwrite = false, bool isEnvi = false);
|
||||
gdalImage BASECONSTVARIABLEAPI CreategdalImage(const QString& img_path, int height, int width, int band_num, Eigen::MatrixXd gt, QString projection,bool need_gt = true, bool overwrite = false, bool isEnvi = false, GDALDataType datetype = GDT_Float32);
|
||||
|
||||
gdalImage BASECONSTVARIABLEAPI CreategdalImage(const QString& img_path, int height, int width, int band_num, Eigen::MatrixXd gt, long espgcode, GDALDataType eType = GDT_Float32, bool need_gt = true, bool overwrite = false, bool isENVI = false);
|
||||
|
||||
|
|
|
|||
|
|
@ -13,143 +13,198 @@
|
|||
|
||||
#include <cmath>
|
||||
#include <stdio.h>
|
||||
#include <cassert>
|
||||
|
||||
#define EPSILON 1e-12
|
||||
#define MAX_ITER 50
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
__device__ __host__ double angleBetweenVectors(Vector3 a, Vector3 b, bool returnDegrees ) {
|
||||
// 计算点积
|
||||
double dotProduct = a.x * b.x + a.y * b.y + a.z * b.z;
|
||||
|
||||
// 计算模长
|
||||
double magA = std::sqrt(a.x * a.x + a.y * a.y + a.z * a.z);
|
||||
double magB = std::sqrt(b.x * b.x + b.y * b.y + b.z * b.z);
|
||||
|
||||
// 处理零向量异常
|
||||
if (magA == 0.0 || magB == 0.0) {
|
||||
return NAN;
|
||||
}
|
||||
|
||||
double cosTheta = dotProduct / (magA * magB);
|
||||
cosTheta = cosTheta < -1 ? -1 : cosTheta>1 ? 1 : cosTheta; // 截断到[-1, 1]
|
||||
double angleRad = std::acos(cosTheta);
|
||||
return returnDegrees ? angleRad * 180.0 / M_PI : angleRad;
|
||||
}
|
||||
|
||||
|
||||
// 向量运算
|
||||
__device__ __host__ Vector3 vec_sub(Vector3 a, Vector3 b) {
|
||||
return { a.x - b.x, a.y - b.y, a.z - b.z };
|
||||
return { a.x - b.x, a.y - b.y, a.z - b.z };
|
||||
}
|
||||
|
||||
__device__ __host__ double vec_dot(Vector3 a, Vector3 b) {
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||||
return a.x * b.x + a.y * b.y + a.z * b.z;
|
||||
}
|
||||
|
||||
__device__ __host__ Vector3 vec_cross(Vector3 a, Vector3 b) {
|
||||
return { a.y * b.z - a.z * b.y,
|
||||
a.z * b.x - a.x * b.z,
|
||||
a.x * b.y - a.y * b.x };
|
||||
return { a.y * b.z - a.z * b.y,
|
||||
a.z * b.x - a.x * b.z,
|
||||
a.x * b.y - a.y * b.x };
|
||||
}
|
||||
|
||||
__device__ __host__ Vector3 vec_normalize(Vector3 v) {
|
||||
double len = sqrt(vec_dot(v, v));
|
||||
return (len > 1e-12) ? Vector3 { v.x / len, v.y / len, v.z / len } : v;
|
||||
double len = sqrt(vec_dot(v, v));
|
||||
return (len > 1e-12) ? Vector3{ v.x / len, v.y / len, v.z / len } : v;
|
||||
}
|
||||
|
||||
|
||||
// 标准正交基底坐标计算
|
||||
__device__ __host__ Vector3 coordinates_orthonormal_basis(Vector3& A,
|
||||
Vector3& e1,
|
||||
Vector3& e2,
|
||||
Vector3& e3) {
|
||||
//// 验证基底正交性和单位长度（容差1e-10）
|
||||
//const double tolerance = 1e-10;
|
||||
//assert(fabs(dot(e1, e2)) < tolerance);
|
||||
//assert(fabs(dot(e1, e3)) < tolerance);
|
||||
//assert(fabs(dot(e2, e3)) < tolerance);
|
||||
//assert(fabs(norm(e1) - 1.0) < tolerance);
|
||||
//assert(fabs(norm(e2) - 1.0) < tolerance);
|
||||
//assert(fabs(norm(e3) - 1.0) < tolerance);
|
||||
|
||||
// 计算投影坐标
|
||||
return Vector3{
|
||||
vec_dot(A, e1),
|
||||
vec_dot(A, e2),
|
||||
vec_dot(A, e3)
|
||||
};
|
||||
}
|
||||
|
||||
|
||||
|
||||
// 计算视线交点T
|
||||
extern __device__ __host__ Vector3 compute_T(Vector3 S, Vector3 ray, double H) {
|
||||
Vector3 dir = vec_normalize(ray);
|
||||
double a_h = WGS84_A + H;
|
||||
Vector3 dir = vec_normalize(ray);
|
||||
double a_h = WGS84_A + H;
|
||||
|
||||
double A = (dir.x * dir.x + dir.y * dir.y) / (a_h * a_h) + dir.z * dir.z / (WGS84_B * WGS84_B);
|
||||
double B = 2.0 * (S.x * dir.x / (a_h * a_h) + S.y * dir.y / (a_h * a_h) + S.z * dir.z / (WGS84_B * WGS84_B));
|
||||
double C = (S.x * S.x + S.y * S.y) / (a_h * a_h) + S.z * S.z / (WGS84_B * WGS84_B) - 1.0;
|
||||
double A = (dir.x * dir.x + dir.y * dir.y) / (a_h * a_h) + dir.z * dir.z / (WGS84_B * WGS84_B); // A > 0
|
||||
double B = 2.0 * (S.x * dir.x / (a_h * a_h) + S.y * dir.y / (a_h * a_h) + S.z * dir.z / (WGS84_B * WGS84_B));
|
||||
double C = (S.x * S.x + S.y * S.y) / (a_h * a_h) + S.z * S.z / (WGS84_B * WGS84_B) - 1.0;
|
||||
|
||||
double disc = B * B - 4 * A * C;
|
||||
if (disc < 0) return Vector3 { NAN, NAN, NAN };
|
||||
double disc = B * B - 4 * A * C;
|
||||
if (disc < 0) return Vector3{ NAN, NAN, NAN };
|
||||
|
||||
double sqrt_d = sqrt(disc);
|
||||
double t = fmax((-B - sqrt_d) / (2 * A), (-B + sqrt_d) / (2 * A));
|
||||
return (t > 1e-6) ? Vector3 { S.x + dir.x * t, S.y + dir.y * t, S.z + dir.z * t }
|
||||
: Vector3 { NAN, NAN, NAN };
|
||||
}
|
||||
|
||||
// 构建平面基底
|
||||
extern __device__ __host__ void compute_basis(Vector3 S, Vector3 T, Vector3* e1, Vector3* e2) {
|
||||
Vector3 ST = vec_normalize(vec_sub(T, S));
|
||||
Vector3 SO = vec_normalize(vec_sub(Vector3 { 0, 0, 0 }, S)); // S->O方向
|
||||
|
||||
*e1 = vec_normalize(vec_cross(ST, SO));
|
||||
*e2 = vec_normalize(vec_cross(*e1, ST));
|
||||
}
|
||||
|
||||
// 牛顿迭代法
|
||||
extern __device__ __host__ int newton_solve(Vector3 S, Vector3 e1, Vector3 e2,
|
||||
double R, double H, double* u, double* v) {
|
||||
double a_h = WGS84_A + H;
|
||||
|
||||
for (int iter = 0; iter < MAX_ITER; ++iter) {
|
||||
Vector3 P = {
|
||||
S.x + e1.x * (*u) + e2.x * (*v),
|
||||
S.y + e1.y * (*u) + e2.y * (*v),
|
||||
S.z + e1.z * (*u) + e2.z * (*v)
|
||||
};
|
||||
|
||||
// 残差计算
|
||||
double f1 = (P.x * P.x + P.y * P.y) / (a_h * a_h) + P.z * P.z / (WGS84_B * WGS84_B) - 1.0;
|
||||
double f2 = (*u) * (*u) + (*v) * (*v) - R * R;
|
||||
if (fabs(f1) < 1e-8 && fabs(f2) < 1e-8) return 1;
|
||||
|
||||
// 雅可比矩阵
|
||||
double J11 = (2 * (S.x + e1.x * (*u) + e2.x * (*v)) * e1.x) / (a_h * a_h)
|
||||
+ (2 * (S.z + e1.z * (*u) + e2.z * (*v)) * e1.z) / (WGS84_B * WGS84_B);
|
||||
double J12 = (2 * (S.x + e1.x * (*u) + e2.x * (*v)) * e2.x) / (a_h * a_h)
|
||||
+ (2 * (S.z + e1.z * (*u) + e2.z * (*v)) * e2.z) / (WGS84_B * WGS84_B);
|
||||
double J21 = 2 * (*u);
|
||||
double J22 = 2 * (*v);
|
||||
|
||||
// 矩阵求逆
|
||||
double det = J11 * J22 - J12 * J21;
|
||||
if (fabs(det) < 1e-12) break;
|
||||
|
||||
double delta_u = (-J22 * f1 + J12 * f2) / det;
|
||||
double delta_v = (J21 * f1 - J11 * f2) / det;
|
||||
|
||||
*u += delta_u;
|
||||
*v += delta_v;
|
||||
}
|
||||
return 0;
|
||||
double sqrt_d = sqrt(disc);
|
||||
double t = fmin((-B - sqrt_d) / (2 * A), (-B + sqrt_d) / (2 * A));// 取最小值
|
||||
return (t > 1e-6) ? Vector3{ S.x + dir.x * t, S.y + dir.y * t, S.z + dir.z * t }
|
||||
: Vector3{ NAN, NAN, NAN };
|
||||
}
|
||||
|
||||
// 主计算函数A
|
||||
extern __device__ __host__ Vector3 compute_P(Vector3 S, Vector3 T, double R, double H) {
|
||||
Vector3 e1, e2;
|
||||
compute_basis(S, T, &e1, &e2);
|
||||
Vector3 ex, ey, ez; // 平面基函数
|
||||
Vector3 ST = vec_normalize(vec_sub(T, S));// S->T
|
||||
Vector3 SO = vec_normalize(vec_sub(Vector3{ 0, 0, 0 }, S)); // S->O
|
||||
Vector3 st1 = vec_sub(T, S);
|
||||
double R0 = sqrt(st1.x * st1.x + st1.y * st1.y + st1.z * st1.z);
|
||||
|
||||
// 计算参考角度方向
|
||||
Vector3 ST_vec = vec_sub(T, S);
|
||||
Vector3 SO_vec = vec_sub(Vector3 { 0, 0, 0 }, S);
|
||||
Vector3 ref_cross = vec_cross(SO_vec, ST_vec);
|
||||
double ref_sign = ref_cross.z; // 取Z分量判断方向
|
||||
// S (Z .) --------Y
|
||||
// |\
|
||||
// | \
|
||||
// | \
|
||||
// X \
|
||||
// | -> T
|
||||
// | /
|
||||
// | /
|
||||
// | /
|
||||
// |/
|
||||
// O
|
||||
|
||||
Vector3 best_P = { NAN, NAN, NAN };
|
||||
double min_dist = INFINITY;
|
||||
|
||||
// 圆周采样
|
||||
const int samples = 36;
|
||||
for (int i = 0; i < samples; ++i) {
|
||||
double angle = 2 * M_PI * i / samples;
|
||||
double u = R * cos(angle);
|
||||
double v = R * sin(angle);
|
||||
ez = vec_normalize(vec_cross(SO, ST)); // Z 轴
|
||||
ey = vec_normalize(vec_cross(ez, SO)); // Y 轴 与 ST 同向 --这个结论在星地几何约束，便是显然的；
|
||||
ex = vec_normalize(SO); //X轴
|
||||
// 大致的理论推导
|
||||
// 这里考虑 成像几何，所以点 P 一定在 ex-0-ey 平面上，所以t3=0;
|
||||
// 定义 SP 的向量与 ex的夹角为 theta , 目标长度为 t
|
||||
// t1=t*cos(Q);
|
||||
// t2=t*sin(Q);
|
||||
// h=(a+H)
|
||||
// Xp=Sx+t1*ex.x+t2*ey.x +t3*ez.x; //因为 t3=0；
|
||||
// Yp=Sy+t1*ex.y+t2*ey.y +t3*ez.y;
|
||||
// Zp=Sz+t1*ex.z+t2*ey.z +t3*ez.z;
|
||||
// Xp^2+Yp^2 Zp^2 Xp^2+Yp^2 Zp^2
|
||||
// ---------- + ------- = 1 ==> ---------- + ------- = 1
|
||||
// (a+H)^2 b^2 h^2 b^2
|
||||
double h2 = (WGS84_A + H) * (WGS84_A + H);
|
||||
double b2 = WGS84_B * WGS84_B;
|
||||
double R2 = R * R;
|
||||
double A = R2 * ((ex.x * ex.x + ex.y * ex.y) / h2 + (ex.z * ex.z) / b2);
|
||||
double B = R2 * ((ex.x * ey.x + ex.y * ey.y) / h2 + (ex.z * ey.z) / b2) * 2;
|
||||
double C = R2 * ((ey.x * ey.x + ey.y * ey.y) / h2 + (ey.z * ey.z) / b2);
|
||||
double D = 1 - ((S.x * S.x + S.y * S.y) / h2 + (S.z * S.z) / b2);
|
||||
double E = 2 * R * ((S.x * ex.x + S.y * ex.y) / h2 + (S.z * ex.z) / b2);
|
||||
double F = 2 * R * ((S.x * ey.x + S.y * ey.y) / h2 + (S.z * ey.z) / b2);
|
||||
|
||||
if (!newton_solve(S, e1, e2, R, H, &u, &v)) continue;
|
||||
// f(Q)=Acos^2(Q)+Bsin(Q)cos(Q)+Csin^2(Q)+E*cos(Q)+F*sin(Q)-D
|
||||
// f'(Q)=(C−A)sin(2Q)+2Bcos(2Q)-Esin(Q)+Fcos(Q)
|
||||
|
||||
Vector3 P = {
|
||||
S.x + e1.x * u + e2.x * v,
|
||||
S.y + e1.y * u + e2.y * v,
|
||||
S.z + e1.z * u + e2.z * v
|
||||
};
|
||||
// 牛顿迭代
|
||||
// f(Q)
|
||||
// Q(t+1)=Q - -----------
|
||||
// f'(Q)
|
||||
|
||||
// 椭球验证
|
||||
double check = (P.x * P.x + P.y * P.y) / ((WGS84_A + H) * (WGS84_A + H))
|
||||
+ P.z * P.z / (WGS84_B * WGS84_B);
|
||||
if (fabs(check - 1.0) > 1e-6) continue;
|
||||
// 求解初始值
|
||||
|
||||
// 角度方向验证
|
||||
Vector3 SP_vec = vec_sub(P, S);
|
||||
Vector3 cur_cross = vec_cross(SP_vec, ST_vec);
|
||||
if (ref_sign * cur_cross.z < 0) continue;
|
||||
double Q0 = angleBetweenVectors(SO, ST, false);
|
||||
double dQ = 0;
|
||||
double fQ = 0;
|
||||
double dfQ = 0;
|
||||
double Q = R < R0 ? Q0 - 1e-3 : Q0 + 1e-3;
|
||||
|
||||
// 选择最近点
|
||||
double dist = vec_dot(vec_sub(P, T), vec_sub(P, T));
|
||||
if (dist < min_dist) {
|
||||
min_dist = dist;
|
||||
best_P = P;
|
||||
}
|
||||
}
|
||||
return best_P;
|
||||
|
||||
// 牛顿迭代法
|
||||
for (int iter = 0; iter < MAX_ITER * 10; ++iter) {
|
||||
fQ = A * cos(Q) * cos(Q) + B * sin(Q) * cos(Q) + C * sin(Q) * sin(Q) + E * cos(Q) + F * sin(Q) - D;
|
||||
dfQ = (C - A) * sin(2 * Q) + B * cos(2 * Q) - E * sin(Q) + F * cos(Q);
|
||||
dQ = fQ / dfQ;
|
||||
if (abs(dQ) < 1e-8) {
|
||||
break;
|
||||
}
|
||||
else {
|
||||
dQ = abs(dQ) < d2r * 2 ? dQ : abs(dQ) / dQ * d2r;
|
||||
Q = Q - dQ;
|
||||
}
|
||||
}
|
||||
|
||||
double t1 = R * cos(Q);
|
||||
double t2 = R * sin(Q);
|
||||
Vector3 P = {
|
||||
S.x + t1 * ex.x + t2 * ey.x, //因为 t3=0；
|
||||
S.y + t1 * ex.y + t2 * ey.y,
|
||||
S.z + t1 * ex.z + t2 * ey.z,
|
||||
|
||||
};
|
||||
|
||||
|
||||
|
||||
// 椭球验证
|
||||
double check = (P.x * P.x + P.y * P.y) / ((WGS84_A + H) * (WGS84_A + H))
|
||||
+ P.z * P.z / (WGS84_B * WGS84_B);
|
||||
if (isnan(Q) || isinf(Q) || fabs(check - 1.0) > 1e-6) {
|
||||
return Vector3{ NAN,NAN,NAN };
|
||||
printf("check value =%f\n", fabs(check - 1.0));
|
||||
}
|
||||
|
||||
return P;
|
||||
}
|
||||
|
||||
|
||||
|
||||
//// 参数校验与主函数
|
||||
//int main() {
|
||||
// Vector3 S = { -2.8e6, -4.2e6, 3.5e6 }; // 卫星位置 (m)
|
||||
|
|
|
|||
|
|
@ -7,22 +7,16 @@
|
|||
#include "GPUTool.cuh"
|
||||
|
||||
|
||||
#define WGS84_A 6378137.0 // ³¤°ëÖá (m)
|
||||
#define WGS84_F (1.0/298.257223563)
|
||||
#define WGS84_B (WGS84_A*(1-WGS84_F)) // ¶Ì°ëÖá (m)
|
||||
#define EPSILON 1e-12
|
||||
#define MAX_ITER 50
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
//extern __device__ __host__ Vector3 vec_sub(Vector3 a, Vector3 b);
|
||||
//extern __device__ __host__ double vec_dot(Vector3 a, Vector3 b);
|
||||
//extern __device__ __host__ Vector3 vec_normalize(Vector3 v);
|
||||
extern __device__ __host__ double angleBetweenVectors(Vector3 a, Vector3 b, bool returnDegrees = false);
|
||||
extern __device__ __host__ Vector3 vec_sub(Vector3 a, Vector3 b);
|
||||
extern __device__ __host__ double vec_dot(Vector3 a, Vector3 b);
|
||||
extern __device__ __host__ Vector3 vec_cross(Vector3 a, Vector3 b);
|
||||
extern __device__ __host__ Vector3 vec_normalize(Vector3 v);
|
||||
extern __device__ __host__ Vector3 vec_normalize(Vector3 v);
|
||||
extern __device__ __host__ Vector3 compute_T(Vector3 S, Vector3 ray_dir, double H);
|
||||
extern __device__ __host__ void compute_basis(Vector3 S, Vector3 T, Vector3* e1, Vector3* e2);
|
||||
extern __device__ __host__ int newton_solve(Vector3 S, Vector3 e1, Vector3 e2, double R, double H, double* u, double* v);
|
||||
extern __device__ __host__ Vector3 compute_P(Vector3 S, Vector3 T, double R, double H);
|
||||
|
||||
|
||||
//
|
||||
extern __device__ __host__ Vector3 compute_P(Vector3 S, Vector3 T, double R, double H );
|
||||
//
|
||||
//
|
||||
|
||||
|
|
|
|||
|
|
@ -1,4 +1,4 @@
|
|||
|
||||
|
||||
|
||||
#include <iostream>
|
||||
#include <memory>
|
||||
|
|
@ -15,9 +15,14 @@
|
|||
#include "GPUBPTool.cuh"
|
||||
|
||||
#ifdef __CUDANVCC___
|
||||
#define EPSILON 1e-12
|
||||
#define MAX_ITER 50
|
||||
|
||||
#ifndef M_PI
|
||||
#define M_PI 3.14159265358979323846
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
__global__ void kernel_TimeBPImageGridNet(double* antPx, double* antPy, double* antPz,
|
||||
|
|
@ -31,55 +36,137 @@ __global__ void kernel_TimeBPImageGridNet(double* antPx, double* antPy, double*
|
|||
long prfid = idx / freqpoints;
|
||||
long Rid = idx % freqpoints;
|
||||
if (idx < pixelcount) {
|
||||
// 计算坐标
|
||||
Vector3 S = { antPx[prfid], antPy[prfid], antPz[prfid] }; // 卫星位置 (m)
|
||||
Vector3 ray = { antDirx[prfid], antDiry[prfid], antDirz[prfid] }; // 视线方向
|
||||
double H = meanH; // 平均高程
|
||||
double R = Rnear + dx * Rid; // 目标距离
|
||||
// 参数校验
|
||||
// 计算坐标
|
||||
Vector3 S = { antPx[prfid], antPy[prfid], antPz[prfid] }; // 卫星位置 (m)
|
||||
Vector3 ray = { antDirx[prfid], antDiry[prfid], antDirz[prfid] }; // 视线方向
|
||||
double H = meanH; // 平均高程
|
||||
double R = Rnear + dx * Rid; // 目标距离
|
||||
// 参数校验
|
||||
if (R <= 0 || H < -WGS84_A * 0.1 || H > WGS84_A * 0.1) {
|
||||
//printf("参数错误：\n H范围：±%.1f km\n R必须>0\n", WGS84_A * 0.1 / 1000);
|
||||
imgx[idx] = 1.0 / 0;
|
||||
imgy[idx] = 1.0 / 0;
|
||||
imgz[idx] = 1.0 / 0;
|
||||
printf("idx=%d;prfid=%d;Rid=%d;S=[%f , %f ,%f ];ray=[%f ,%f ,%f ];H=%f;R=%f,imgP=[%f ,%f , %f ];Rextend\n",
|
||||
idx, prfid, Rid, S.x, S.y, S.z, ray.x, ray.y, ray.z, H, R,imgx[idx],imgy[idx],imgz[idx]);
|
||||
// 参数校验
|
||||
//printf("参数错误：\n H范围：±%.1f km\n R必须>0\n", WGS84_A * 0.1 / 1000);
|
||||
imgx[idx] = NAN;
|
||||
imgy[idx] = NAN;
|
||||
imgz[idx] = NAN;
|
||||
//printf("idx=%d;prfid=%d;Rid=%d;S=[%f , %f ,%f ];ray=[%f ,%f ,%f ];H=%f;R=%f,imgP=[%f ,%f , %f ];Rextend\n",
|
||||
// idx, prfid, Rid, S.x, S.y, S.z, ray.x, ray.y, ray.z, H, R,imgx[idx],imgy[idx],imgz[idx]);
|
||||
// 参数校验
|
||||
return;
|
||||
}
|
||||
|
||||
// Step 1: 计算交点T
|
||||
// Step 1: 计算交点T
|
||||
Vector3 T = compute_T(S, ray, H);
|
||||
if (isnan(T.x)) {
|
||||
imgx[idx] = 1.0 / 0;
|
||||
imgy[idx] = 1.0 / 0;
|
||||
imgz[idx] = 1.0 / 0;
|
||||
printf("idx=%d;prfid=%d;Rid=%d;S=[%f , %f ,%f ];ray=[%f ,%f ,%f ];H=%f;R=%f;imgT=[%f ,%f ,%f ];imgP=[%f ,%f , %f ];Tnan\n",
|
||||
idx, prfid, Rid, S.x, S.y, S.z, ray.x, ray.y, ray.z, H, R,T.x,T.y,T.z, imgx[idx], imgy[idx], imgz[idx]);
|
||||
imgx[idx] = NAN;
|
||||
imgy[idx] = NAN;
|
||||
imgz[idx] = NAN;
|
||||
//printf("idx=%d;prfid=%d;Rid=%d;Tnan\n",
|
||||
// idx, prfid, Rid, S.x, S.y, S.z, ray.x, ray.y, ray.z, H, R,T.x,T.y,T.z, imgx[idx], imgy[idx], imgz[idx]);
|
||||
return;
|
||||
}
|
||||
|
||||
// Step 2: 计算目标点P
|
||||
Vector3 P = compute_P(S, T, R, H);
|
||||
// Step 2: 计算目标点P
|
||||
|
||||
Vector3 P;// = compute_P(S, T, R, H);
|
||||
{ // 计算P
|
||||
Vector3 ex, ey, ez; // 平面基函数
|
||||
Vector3 ST = vec_normalize(vec_sub(T, S));// S->T
|
||||
Vector3 SO = vec_normalize(vec_sub(Vector3{ 0, 0, 0 }, S)); // S->O
|
||||
|
||||
if (!isnan(P.x)) {
|
||||
|
||||
Vector3 st1 = vec_sub(T, S);
|
||||
double R0 = sqrt(st1.x * st1.x + st1.y * st1.y + st1.z * st1.z);
|
||||
ez = vec_normalize(vec_cross(SO, ST)); // Z 轴
|
||||
ey = vec_normalize(vec_cross(ez, SO)); // Y 轴 与 ST 同向 --这个结论在星地几何约束，便是显然的；
|
||||
ex = vec_normalize(SO); //X轴
|
||||
|
||||
|
||||
|
||||
double h2 = (WGS84_A + H) * (WGS84_A + H);
|
||||
double b2 = WGS84_B * WGS84_B;
|
||||
double R2 = R * R;
|
||||
double A = R2 * ((ex.x * ex.x + ex.y * ex.y) / h2 + (ex.z * ex.z) / b2);
|
||||
double B = R2 * ((ex.x * ey.x + ex.y * ey.y) / h2 + (ex.z * ey.z) / b2) * 2;
|
||||
double C = R2 * ((ey.x * ey.x + ey.y * ey.y) / h2 + (ey.z * ey.z) / b2);
|
||||
double D = 1 - ((S.x * S.x + S.y * S.y) / h2 + (S.z * S.z) / b2);
|
||||
double E = 2*R * ((S.x * ex.x + S.y * ex.y) / h2 + (S.z * ex.z) / b2);
|
||||
double F = 2*R * ((S.x * ey.x + S.y * ey.y) / h2 + (S.z * ey.z) / b2);
|
||||
double Q0 = angleBetweenVectors(SO, ST, false);
|
||||
double dQ = 0;
|
||||
double fQ = 0;
|
||||
double dfQ = 0;
|
||||
double Q = R < R0 ? Q0 - 1e-3 : Q0 + 1e-3;
|
||||
|
||||
//printf("A=%f;B=%f;C=%f;D=%f;E=%f;F=%f;Q=%f;\
|
||||
// S=[%f , %f ,%f ];\
|
||||
// T=[%f , %f ,%f ];\
|
||||
// ex=[%f , %f ,%f ];\
|
||||
// ey=[%f , %f ,%f ];\
|
||||
// ez=[%f , %f ,%f ];\
|
||||
//ray=[%f ,%f ,%f ];\
|
||||
//H=%f;R=%f;;\n",A,B,C,D,E,F,Q,
|
||||
// S.x,S.y,S.z,
|
||||
// T.x,T.y,T.z ,
|
||||
// ex.x,ex.y,ex.z,
|
||||
// ey.x,ey.y,ey.z,
|
||||
// ez.x,ez.y,ez.z,
|
||||
// ray.x, ray.y, ray.z,
|
||||
// H, R);
|
||||
// return;
|
||||
|
||||
// 牛顿迭代法
|
||||
for (int iter = 0; iter < MAX_ITER * 10; ++iter) {
|
||||
fQ = A * cos(Q) * cos(Q) + B * sin(Q) * cos(Q) + C * sin(Q) * sin(Q) + E * cos(Q) + F * sin(Q) - D;
|
||||
dfQ = (C - A) * sin(2 * Q) + B * cos(2 * Q) - E * sin(Q) + F * cos(Q);
|
||||
dQ = fQ / dfQ;
|
||||
if (abs(dQ) < 1e-8) {
|
||||
//printf("iter=%d;check Q0=%f;Q=%f;dQ=%f;fQ=%f;dfQ=%f;break\n", iter, Q0, Q, dQ, fQ, dfQ);
|
||||
break;
|
||||
}
|
||||
else {
|
||||
dQ = (abs(dQ) < d2r * 3) ? dQ :( abs(dQ) / dQ * d2r* 3);
|
||||
Q = Q - dQ;
|
||||
//printf("iter=%d;check Q0=%f;Q=%f;dQ=%f;fQ=%f;dfQ=%f;\n", iter, Q0, Q, dQ, fQ, dfQ);
|
||||
}
|
||||
|
||||
}
|
||||
//printf("check Q0=%f;Q=%f;\n", Q0, Q);
|
||||
double t1 = R * cos(Q);
|
||||
double t2 = R * sin(Q);
|
||||
P = Vector3{
|
||||
S.x + t1 * ex.x + t2 * ey.x, //因为 t3=0；
|
||||
S.y + t1 * ex.y + t2 * ey.y,
|
||||
S.z + t1 * ex.z + t2 * ey.z,
|
||||
};
|
||||
double check = (P.x * P.x + P.y * P.y) / ((WGS84_A + H) * (WGS84_A + H))
|
||||
+ P.z * P.z / (WGS84_B * WGS84_B);
|
||||
if (isnan(Q) || isinf(Q) || fabs(check - 1.0) > 1e-6) {
|
||||
P = Vector3{ NAN,NAN,NAN };
|
||||
}
|
||||
}
|
||||
|
||||
double Rt = sqrt(pow(S.x - T.x, 2) + pow(S.y - T.y, 2) + pow(S.z - T.z, 2));
|
||||
double Rp = sqrt(pow(S.x - P.x, 2) + pow(S.y - P.y, 2) + pow(S.z - P.z, 2));
|
||||
double Rop = sqrt(pow( P.x, 2) + pow( P.y, 2) + pow( P.z, 2));
|
||||
|
||||
if (!isnan(P.x)&&( Rop>WGS84_A*0.3)&&(Rop<WGS84_A*3)) {
|
||||
|
||||
imgx[idx] = P.x;
|
||||
imgy[idx] = P.y;
|
||||
imgz[idx] = P.z;
|
||||
printf("idx=%d;prfid=%d;Rid=%d;S=[%f , %f ,%f ];ray=[%f ,%f ,%f ];H=%f;R=%f;imgT=[%f ,%f ,%f ];imgP=[%f ,%f , %f ]; \n",
|
||||
idx, prfid, Rid, S.x, S.y, S.z, ray.x, ray.y, ray.z, H, R, T.x, T.y, T.z, imgx[idx], imgy[idx], imgz[idx]);
|
||||
|
||||
//printf("idx=%d; S=[%f , %f ,%f ]; H=%f;R=%f;RP=%f;Rr=%f;imgT=[%f ,%f ,%f ];imgP=[%f ,%f , %f ]; \n",
|
||||
// idx, S.x, S.y, S.z, H, R, Rp, Rt,T.x, T.y, T.z, P.x, P.y, P.z);
|
||||
}
|
||||
else {
|
||||
imgx[idx] = 1.0 / 0;
|
||||
imgy[idx] = 1.0 / 0;
|
||||
imgz[idx] = 1.0 / 0;
|
||||
printf("idx=%d;prfid=%d;Rid=%d;S=[%f , %f ,%f ];ray=[%f ,%f ,%f ];H=%f;R=%f;imgT=[%f ,%f ,%f ];imgP=[%f ,%f , %f ];Pnan\n",
|
||||
idx, prfid, Rid, S.x, S.y, S.z, ray.x, ray.y, ray.z, H, R, T.x, T.y, T.z, imgx[idx], imgy[idx], imgz[idx]);
|
||||
//printf("未找到有效解\n");
|
||||
imgx[idx] = NAN;
|
||||
imgy[idx] = NAN;
|
||||
imgz[idx] = NAN;
|
||||
printf("idx=%d; S=[%f , %f ,%f ]; H=%f;R=%f;RP=%f;Rr=%f;imgT=[%f ,%f ,%f ];imgP=[%f ,%f , %f ]; ERROR\n",
|
||||
idx, S.x, S.y, S.z, H, R, Rp, Rt, T.x, T.y, T.z, P.x, P.y, P.z);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
|
@ -121,7 +208,7 @@ __global__ void kernel_pixelTimeBP(
|
|||
Pz = antPz[pid];
|
||||
|
||||
PR = sqrt((Px - Tx) * (Px - Tx) + (Py - Ty) * (Py - Ty) + (Pz - Tz) * (Pz - Tz));
|
||||
Rid = (PR - Rnear) / dx; // 行数
|
||||
Rid = (PR - Rnear) / dx; // 行数
|
||||
|
||||
if (Rid<0 || Rid>maxPointIdx) {
|
||||
continue;
|
||||
|
|
@ -135,14 +222,14 @@ __global__ void kernel_pixelTimeBP(
|
|||
continue;
|
||||
}
|
||||
else {}
|
||||
// 线性插值
|
||||
// 线性插值
|
||||
s0 = TimeEchoArr[pid0];
|
||||
s1 = TimeEchoArr[pid1];
|
||||
|
||||
s.x = s0.x * (Rid - pid0) + s1.x * (pid1 - Rid);
|
||||
s.y = s0.y * (Rid - pid0) + s1.y * (pid1 - Rid);
|
||||
|
||||
// 相位校正
|
||||
// 相位校正
|
||||
|
||||
phi = 4 * LIGHTSPEED/startLamda* (PR - RefRange) ; // 4PI/lamda * R
|
||||
|
||||
|
|
@ -150,7 +237,7 @@ __global__ void kernel_pixelTimeBP(
|
|||
phiCorr.x = cos(phi);
|
||||
phiCorr.y = sin(phi);
|
||||
|
||||
s = cuCmulf(s, phiCorr); // 校正后
|
||||
s = cuCmulf(s, phiCorr); // 校正后
|
||||
|
||||
imgArr[idx] = cuCaddf(imgArr[idx], s);
|
||||
}
|
||||
|
|
@ -195,7 +282,7 @@ extern "C" {
|
|||
long pixelcount = imH * imW;
|
||||
int grid_size = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;
|
||||
|
||||
kernel_pixelTimeBP << <grid_size, BLOCK_SIZE >> > (
|
||||
kernel_pixelTimeBP << <1, 1 >> > (
|
||||
antPx, antPy, antPz,
|
||||
imgx, imgy, imgz,
|
||||
TimeEchoArr, prfcount, pointCount,
|
||||
|
|
|
|||
|
|
@ -45,7 +45,7 @@
|
|||
</size>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>D:\Programme\vs2022\RasterMergeTest\LAMPCAE_SCANE-all-scane\GF3_Simulation.xml</string>
|
||||
<string>D:\Programme\vs2022\RasterMergeTest\LAMPCAE_SCANE\GF3_Simulation.xml</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
|
|
@ -96,7 +96,7 @@
|
|||
</size>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>D:\Programme\vs2022\RasterMergeTest\LAMPCAE_SCANE-all-scane\BPImage\GF3BPImage</string>
|
||||
<string>D:\Programme\vs2022\RasterMergeTest\LAMPCAE_SCANE\BPImage\GF3BPImage</string>
|
||||
</property>
|
||||
</widget>
|
||||
</item>
|
||||
|
|
|
|||
|
|
@ -29,7 +29,7 @@ void CreatePixelXYZ(std::shared_ptr<EchoL0Dataset> echoL0ds, QString outPixelXYZ
|
|||
gt(1, 0) = 0;
|
||||
gt(1, 1) = 0;
|
||||
gt(1, 2) = 1;
|
||||
gdalImage xyzRaster = CreategdalImage(outPixelXYZPath, prfcount, freqcount, 3, gt, QString(""), false, true,true);
|
||||
gdalImage xyzRaster = CreategdalImage(outPixelXYZPath, prfcount, freqcount, 3, gt, QString(""), false, true,true,GDT_Float64);
|
||||
std::shared_ptr<double> antpos = echoL0ds->getAntPos();
|
||||
dx = (echoL0ds->getFarRange()-echoL0ds->getNearRange())/(echoL0ds->getPlusePoints()-1);
|
||||
Rnear = echoL0ds->getNearRange();
|
||||
|
|
@ -125,7 +125,7 @@ void CreatePixelXYZ(std::shared_ptr<EchoL0Dataset> echoL0ds, QString outPixelXYZ
|
|||
d_Pxs.get(), d_Pys.get(), d_Pzs.get(),
|
||||
d_AntDirectX.get(), d_AntDirectY.get(), d_AntDirectZ.get(),
|
||||
d_demx.get(), d_demy.get(), d_demz.get(),
|
||||
prfcount, tempechocol, 0,
|
||||
prfcount, tempechocol, 1000,
|
||||
Rnear, dx, refRange
|
||||
);
|
||||
|
||||
|
|
@ -210,9 +210,7 @@ ErrorCode TBPImageAlgCls::Process(long num_thread)
|
|||
{
|
||||
|
||||
qDebug() << u8"开始成像";
|
||||
qDebug() << u8"频域回波-> 时域回波";
|
||||
this->TimeEchoDataPath = JoinPath(this->L1ds->getoutFolderPath(), this->L0ds->getSimulationTaskName() + "_Timeecho.bin");
|
||||
this->EchoFreqToTime();
|
||||
|
||||
|
||||
qDebug() << u8"创建成像平面的XYZ";
|
||||
QString outRasterXYZ = JoinPath(this->L1ds->getoutFolderPath(), this->L0ds->getSimulationTaskName() + "_xyz.bin");
|
||||
|
|
@ -220,6 +218,10 @@ ErrorCode TBPImageAlgCls::Process(long num_thread)
|
|||
this->outRasterXYZPath = outRasterXYZ;
|
||||
|
||||
|
||||
qDebug() << u8"频域回波-> 时域回波";
|
||||
this->TimeEchoDataPath = JoinPath(this->L1ds->getoutFolderPath(), this->L0ds->getSimulationTaskName() + "_Timeecho.bin");
|
||||
this->EchoFreqToTime();
|
||||
|
||||
// 初始化Raster
|
||||
qDebug() << u8"初始化影像";
|
||||
long imageheight = this->L1ds->getrowCount();
|
||||
|
|
|
|||
|
|
@ -228,7 +228,7 @@
|
|||
<QtMoc Include="SimulationSAR\QSARLookTableSimualtionGUI.h" />
|
||||
<QtMoc Include="SimulationSAR\QSimulationBPImage.h" />
|
||||
<QtMoc Include="SimulationSAR\QSimulationRFPCGUI.h" />
|
||||
<ClInclude Include="SimulationSAR\GPUBPTool.cuh" />
|
||||
<CudaCompile Include="SimulationSAR\GPUBPTool.cuh" />
|
||||
<ClInclude Include="SimulationSAR\RFPCProcessCls.h" />
|
||||
<ClInclude Include="SimulationSAR\SARSatelliteSimulationAbstractCls.h" />
|
||||
<ClInclude Include="SimulationSAR\SARSimulationTaskSetting.h" />
|
||||
|
|
|
|||
|
|
@ -59,9 +59,6 @@
|
|||
<ClInclude Include="PowerSimulationIncoherent\OribtModelOperator.h">
|
||||
<Filter>PowerSimulationIncoherent</Filter>
|
||||
</ClInclude>
|
||||
<ClInclude Include="SimulationSAR\GPUBPTool.cuh">
|
||||
<Filter>SimulationSAR</Filter>
|
||||
</ClInclude>
|
||||
</ItemGroup>
|
||||
<ItemGroup>
|
||||
<ClCompile Include="SimulationSAR\QImageSARRFPC.cpp">
|
||||
|
|
@ -178,5 +175,8 @@
|
|||
<CudaCompile Include="SimulationSAR\GPUBPTool.cu">
|
||||
<Filter>SimulationSAR</Filter>
|
||||
</CudaCompile>
|
||||
<CudaCompile Include="SimulationSAR\GPUBPTool.cuh">
|
||||
<Filter>SimulationSAR</Filter>
|
||||
</CudaCompile>
|
||||
</ItemGroup>
|
||||
</Project>
|
||||
Loading…
Reference in New Issue