457 lines
12 KiB
Plaintext
457 lines
12 KiB
Plaintext
|
||
|
||
#include <iostream>
|
||
#include <memory>
|
||
#include <cmath>
|
||
#include <complex>
|
||
#include <device_launch_parameters.h>
|
||
#include <cuda_runtime.h>
|
||
#include <cublas_v2.h>
|
||
#include <cuComplex.h>
|
||
|
||
#include "BaseConstVariable.h"
|
||
#include "GPUTool.cuh"
|
||
#include "GPUTBPImage.cuh"
|
||
#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,
|
||
double* antDirx, double* antDiry, double* antDirz,
|
||
double* imgx, double* imgy, double* imgz,
|
||
long prfcount, long freqpoints, double meanH,
|
||
double Rnear, double dx, double RefRange) {
|
||
|
||
long idx = blockIdx.x * blockDim.x + threadIdx.x;
|
||
long pixelcount = prfcount * freqpoints;
|
||
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; // 目标距离
|
||
// 参数校验
|
||
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] = 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
|
||
Vector3 T = compute_T(S, ray, H);
|
||
if (isnan(T.x)) {
|
||
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);
|
||
{ // 计算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
|
||
|
||
|
||
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; 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] = 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);
|
||
|
||
}
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
__device__ double computerR(double& Px, double& Py, double& Pz, double& Tx, double& Ty, double& Tz) {
|
||
//double R= sqrt((Px - Tx) * (Px - Tx) + (Py - Ty) * (Py - Ty) + (Pz - Tz) * (Pz - Tz));
|
||
//if (R > 900000) {
|
||
// printf("R=%f\n", R);
|
||
//}
|
||
//return R;
|
||
|
||
return sqrt((Px - Tx) * (Px - Tx) + (Py - Ty) * (Py - Ty) + (Pz - Tz) * (Pz - Tz));
|
||
}
|
||
|
||
|
||
|
||
__device__ void updateBPImage(
|
||
long prfid, long pixelidx,double R,double LRrange,
|
||
cuComplex* TimeEchoArr, long prfcount, long pointCount,
|
||
cuComplex* imgArr,
|
||
double startLamda, double Rnear, double dx, double RefRange, double Rfar
|
||
) {
|
||
double ridx = (R - LRrange) / dx; // 获取范围
|
||
if (ridx < 0 || ridx >= pointCount) {
|
||
return;
|
||
}
|
||
else {}
|
||
|
||
long Ridx0 = floor(ridx);
|
||
long Ridx1 = ceil(ridx);
|
||
|
||
long pid0 = prfid * pointCount + Ridx0;
|
||
long pid1 = prfid * pointCount + Ridx1;
|
||
|
||
cuComplex s0 = TimeEchoArr[pid0];
|
||
cuComplex s1 = TimeEchoArr[pid1];
|
||
|
||
|
||
if (isinf(s0.x) || isinf(s0.y) || isinf(s1.x) || isinf(s1.y)) {
|
||
return;
|
||
}
|
||
|
||
cuComplex s = make_cuComplex(
|
||
s0.x + (s1.x - s0.x) * (ridx-Ridx0), // real
|
||
s0.y + (s1.y - s0.y) * (ridx-Ridx0) // imag
|
||
);
|
||
double phi = 4 * PI / startLamda * (R - RefRange);
|
||
|
||
// exp(ix)=cos(x)+isin(x)
|
||
cuComplex phiCorr = make_cuComplex(cos(phi), sin(phi));
|
||
s = cuCmulf(s, phiCorr); // 校正后
|
||
|
||
imgArr[pixelidx] = cuCaddf(imgArr[pixelidx], s);
|
||
//imgArr[pixelidx] = cuCaddf(imgArr[pixelidx], make_cuComplex(1,1));
|
||
return;
|
||
}
|
||
|
||
|
||
|
||
|
||
// 分块计算
|
||
__device__ void segmentBPImage(
|
||
double* antPx, double* antPy, double* antPz,
|
||
double Tx, double Ty, double Tz,
|
||
cuComplex* TimeEchoArr, long prfcount, long pointCount,
|
||
cuComplex* imgArr,
|
||
double startLamda, double Rnear, double dx, double RefRange, double Rfar,
|
||
long startSegmentPrfId,long pixelID // 分段起始prfid
|
||
) {
|
||
// 计算单条脉冲范围
|
||
double Rrange = pointCount * dx;// 成像范围
|
||
double LRrange = RefRange - Rrange / 2;//左范围
|
||
double RRrange = RefRange + Rrange / 2;
|
||
|
||
long currentprfid = 0;
|
||
// 0
|
||
currentprfid = startSegmentPrfId + 0;
|
||
double Px = antPx[currentprfid];
|
||
double Py = antPy[currentprfid];
|
||
double Pz = antPz[currentprfid];
|
||
double R0 = computerR(Px, Py, Pz, Tx, Ty, Tz);
|
||
|
||
|
||
if (LRrange <= R0 && R0 <= RRrange) {
|
||
updateBPImage(
|
||
currentprfid, pixelID, R0, LRrange,
|
||
TimeEchoArr, prfcount, pointCount,
|
||
imgArr,
|
||
startLamda, Rnear, dx, RefRange, Rfar
|
||
);
|
||
}
|
||
|
||
// 10
|
||
currentprfid = startSegmentPrfId + 10;
|
||
Px = antPx[currentprfid];
|
||
Py = antPy[currentprfid];
|
||
Pz = antPz[currentprfid];
|
||
double R10 = computerR(Px, Py, Pz, Tx, Ty, Tz);
|
||
if (Rnear <= R10 && R10 <= RRrange) {
|
||
updateBPImage(
|
||
currentprfid, pixelID, R10, LRrange,
|
||
TimeEchoArr, prfcount, pointCount,
|
||
imgArr,
|
||
startLamda, Rnear, dx, RefRange, Rfar
|
||
);
|
||
}
|
||
|
||
|
||
//19
|
||
currentprfid = startSegmentPrfId + 19;
|
||
Px = antPx[currentprfid];
|
||
Py = antPy[currentprfid];
|
||
Pz = antPz[currentprfid];
|
||
double R19 = computerR(Px, Py, Pz, Tx, Ty, Tz);
|
||
if (Rnear <= R19 && R19 <= RRrange) {
|
||
updateBPImage(
|
||
currentprfid, pixelID, R19, LRrange,
|
||
TimeEchoArr, prfcount, pointCount,
|
||
imgArr,
|
||
startLamda, Rnear, dx, RefRange, Rfar
|
||
);
|
||
}
|
||
|
||
// 判断是否需要处理
|
||
|
||
if (R0 < LRrange && R10 < LRrange && R19 < LRrange ) { // 越界、不处理
|
||
return;
|
||
}
|
||
else if (R0 > RRrange && R10 > RRrange && R19 > RRrange) {// 越界、不处理
|
||
return;
|
||
}
|
||
else {}
|
||
|
||
|
||
|
||
|
||
double R = 0;
|
||
#pragma unroll
|
||
for (long i = 1; i < 10; i++) {
|
||
currentprfid = startSegmentPrfId + i;
|
||
if (currentprfid < prfcount) {
|
||
Px = antPx[currentprfid];
|
||
Py = antPy[currentprfid];
|
||
Pz = antPz[currentprfid];
|
||
R = computerR(Px, Py, Pz, Tx, Ty, Tz);
|
||
updateBPImage(
|
||
currentprfid, pixelID, R, LRrange,
|
||
TimeEchoArr, prfcount, pointCount,
|
||
imgArr,
|
||
startLamda, Rnear, dx, RefRange, Rfar
|
||
);
|
||
}
|
||
}
|
||
|
||
#pragma unroll
|
||
for (long i = 11; i < 20; i++) {
|
||
currentprfid = startSegmentPrfId + i;
|
||
if (currentprfid < prfcount) {
|
||
Px = antPx[currentprfid];
|
||
Py = antPy[currentprfid];
|
||
Pz = antPz[currentprfid];
|
||
R = computerR(Px, Py, Pz, Tx, Ty, Tz);
|
||
updateBPImage(
|
||
currentprfid, pixelID, R, LRrange,
|
||
TimeEchoArr, prfcount, pointCount,
|
||
imgArr,
|
||
startLamda, Rnear, dx, RefRange, Rfar
|
||
);
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
__global__ void kernel_pixelTimeBP(
|
||
double* antPx, double* antPy, double* antPz,
|
||
double* imgx, double* imgy, double* imgz,
|
||
cuComplex* TimeEchoArr, long prfcount, long pointCount,
|
||
cuComplex* imgArr, long imH, long imW,
|
||
double startLamda, double Rnear, double dx,double RefRange,double Rfar
|
||
) {
|
||
long idx = blockIdx.x * blockDim.x + threadIdx.x;
|
||
long pixelcount = imH * imW;
|
||
if (idx < pixelcount) {
|
||
|
||
double Tx = imgx[idx]; // 地面坐标点
|
||
double Ty = imgy[idx];
|
||
double Tz = imgz[idx];
|
||
double Rrange = pointCount * dx;// 成像范围
|
||
double LRrange = RefRange - Rrange / 2;//左范围
|
||
double RRrange = RefRange + Rrange / 2;
|
||
|
||
for (long segid = 0; segid < prfcount; segid = segid + 20) {
|
||
long seglen = prfcount - segid;
|
||
if (seglen < 20) {
|
||
for (long i = 1; i < 10; i++) {
|
||
long currentprfid = segid + i;
|
||
if (currentprfid < prfcount) {
|
||
double Px = antPx[currentprfid];
|
||
double Py = antPy[currentprfid];
|
||
double Pz = antPz[currentprfid];
|
||
double R = computerR(Px, Py, Pz, Tx, Ty, Tz);
|
||
updateBPImage(
|
||
currentprfid, idx, R, LRrange,
|
||
TimeEchoArr, prfcount, pointCount,
|
||
imgArr,
|
||
startLamda, Rnear, dx, RefRange, Rfar
|
||
);
|
||
}
|
||
}
|
||
}
|
||
else {
|
||
// 判断范围
|
||
segmentBPImage(
|
||
antPx, antPy, antPz,
|
||
Tx, Ty, Tz,
|
||
TimeEchoArr, prfcount, pointCount,
|
||
imgArr,
|
||
startLamda, Rnear, dx, RefRange, Rfar,
|
||
segid, idx
|
||
);
|
||
}
|
||
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
extern "C" {
|
||
void TIMEBPCreateImageGrid(double* antPx, double* antPy, double* antPz,
|
||
double* antDirx, double* antDiry, double* antDirz,
|
||
double* imgx, double* imgy, double* imgz,
|
||
long prfcount, long freqpoints, double meanH,
|
||
double Rnear, double dx, double RefRange
|
||
)
|
||
{
|
||
long pixelcount = prfcount * freqpoints;
|
||
int grid_size = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;
|
||
|
||
kernel_TimeBPImageGridNet << <grid_size, BLOCK_SIZE >> > (
|
||
antPx, antPy, antPz,
|
||
antDirx, antDiry, antDirz,
|
||
imgx, imgy, imgz,
|
||
prfcount, freqpoints, meanH,
|
||
Rnear, dx, RefRange);
|
||
PrintLasterError("TIMEBPCreateImageGrid");
|
||
cudaDeviceSynchronize();
|
||
}
|
||
|
||
|
||
|
||
|
||
void TimeBPImage(
|
||
double* antPx, double* antPy, double* antPz,
|
||
double* imgx, double* imgy, double* imgz,
|
||
cuComplex* TimeEchoArr, long prfcount, long pointCount,
|
||
cuComplex* imgArr, long imH, long imW,
|
||
double startLamda, double Rnear, double dx, double RefRange,double Rfar
|
||
)
|
||
{
|
||
long pixelcount = imH * imW;
|
||
int grid_size = (pixelcount + BLOCK_SIZE - 1) / BLOCK_SIZE;
|
||
|
||
kernel_pixelTimeBP << <grid_size, BLOCK_SIZE >> > (
|
||
antPx, antPy, antPz,
|
||
imgx, imgy, imgz,
|
||
TimeEchoArr, prfcount, pointCount,
|
||
imgArr, imH, imW,
|
||
startLamda, Rnear, dx, RefRange, Rfar
|
||
);
|
||
|
||
PrintLasterError("TimeBPImage");
|
||
cudaDeviceSynchronize();
|
||
|
||
}
|
||
|
||
}
|
||
|
||
#endif
|
||
|
||
|