#include "stdafx.h" //#define EIGEN_USE_BLAS #define EIGEN_VECTORIZE_SSE4_2 //#include #include #include #include #include ////#include #include #include #include < io.h > #include < stdio.h > #include < stdlib.h > #include #include #include //#include #include //#include "ogrsf_frmts.h" #include #include #include "GeoOperator.h" #include #include #include "baseTool.h" #include // 多项式拟合 #include /* 提供了 gammaq 函数 */ #include /* 提供了向量结构*/ #include #include #include #include // 包含SSE指令集头文件 #include // 包含SSE2指令集头文件 #include // 包含OpenMP头文件 #include #include #include #include QString PolarTypeEnumToString(POLARTYPEENUM type) { switch (type) { case POLARHH: return "HH"; case POLARHV: return "HV"; case POLARVH: return "VH"; case POLARVV: return "VV"; default: return "UNKOWN"; } } POLARTYPEENUM StringToPolarTypeEnum(const QString& str) { if (str.compare("HH", Qt::CaseInsensitive) == 0) return POLARHH; if (str.compare("HV", Qt::CaseInsensitive) == 0) return POLARHV; if (str.compare("VH", Qt::CaseInsensitive) == 0) return POLARVH; if (str.compare("VV", Qt::CaseInsensitive) == 0) return POLARVV; return POLARUNKOWN; } QString longDoubleToQStringScientific(long double value) { std::ostringstream stream; // 设置流的精度为35位有效数字,并使用科学计数法 stream << std::scientific << std::setprecision(35) << value; // 将标准字符串转换为 QString return QString::fromStdString(stream.str()); } QString getCurrentTimeString() { struct tm ConversionTime; std::time_t t = std::time(NULL); char mbstr[100]; _localtime64_s(&ConversionTime, &t); std::strftime(mbstr, sizeof(mbstr), "%Y-%m-%d %H:%M:%S", &ConversionTime); QString strTime = mbstr; return strTime; } QString getCurrentShortTimeString() { struct tm ConversionTime; std::time_t t = std::time(NULL); char mbstr[100]; _localtime64_s(&ConversionTime, &t); std::strftime(mbstr, sizeof(mbstr), "%Y-%m-%d %H:%M:%S", &ConversionTime); QString strTime = mbstr; return strTime; } std::vector splitString(const QString& str, char delimiter) { QStringList tokens = str.split(delimiter); return convertQStringListToStdVector(tokens); } std::complex Cubic_Convolution_interpolation(double u, double v, Eigen::MatrixX> img) { if (img.rows() != 4 || img.cols() != 4) { throw std::exception("the size of img's block is not right"); } // 斤拷锟斤拷模锟斤拷 Eigen::MatrixX> wrc(1, 4);// 使锟斤拷 complex 斤拷锟斤拷要原斤拷为锟剿伙拷取值 Eigen::MatrixX> wcr(4, 1);// for (int i = 0; i < 4; i++) { wrc(0, i) = Cubic_kernel_weight(u + 1 - i); // u+1,u,u-1,u-2 wcr(i, 0) = Cubic_kernel_weight(v + 1 - i); } Eigen::MatrixX> interValue = wrc * img * wcr; return interValue(0, 0); } std::complex Cubic_kernel_weight(double s) { s = abs(s); if (s <= 1) { return std::complex(1.5 * s * s * s - 2.5 * s * s + 1, 0); } else if (s <= 2) { return std::complex(-0.5 * s * s * s + 2.5 * s * s - 4 * s + 2, 0); } else { return std::complex(0, 0); } } /// /// p11 p12 -- x /// p0(u,v) /// p21 p22 /// | /// y /// p11(0,0) /// p21(0,1) /// P12(1,0) /// p22(1,1) /// /// x,y,z /// x,y,z /// x,y,z /// x,y,z /// x,y,z /// double Bilinear_interpolation(Landpoint p0, Landpoint p11, Landpoint p21, Landpoint p12, Landpoint p22) { return p11.ati * (1 - p0.lon) * (1 - p0.lat) + p12.ati * p0.lon * (1 - p0.lat) + p21.ati * (1 - p0.lon) * p0.lat + p22.ati * p0.lon * p0.lat; } bool onSegment(Point3 Pi, Point3 Pj, Point3 Q) { if ((Q.x - Pi.x) * (Pj.y - Pi.y) == (Pj.x - Pi.x) * (Q.y - Pi.y) //叉乘 //保证Q点坐标在pi,pj之间 && std::min(Pi.x, Pj.x) <= Q.x && Q.x <= std::max(Pi.x, Pj.x) && std::min(Pi.y, Pj.y) <= Q.y && Q.y <= std::max(Pi.y, Pj.y)) return true; else return false; } Point3 invBilinear(Point3 p, Point3 a, Point3 b, Point3 c, Point3 d) { Point3 res; Point3 e = b - a; Point3 f = d - a; Point3 g = a - b + c - d; Point3 h = p - a; double k2 = cross2d(g, f); double k1 = cross2d(e, f) + cross2d(h, g); double k0 = cross2d(h, e); double u, v; // if edges are parallel, this is a linear equation if (abs(k2) < 0.001) { v = -k0 / k1; u = (h.x - f.x * v) / (e.x + g.x * v); p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v; return p; } // otherwise, it's a quadratic else { float w = k1 * k1 - 4.0 * k0 * k2; if (w < 0.0) { // 可能在边界上 if (onSegment(a, b, p)) { Point3 tt = b - a; Point3 ttpa = p - a; double scater = ttpa / tt; if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; } p.z = a.z + scater * tt.z; return p; } else if (onSegment(b, c, p)) { Point3 tt = c - b; Point3 ttpa = p - b; double scater = ttpa / tt; if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; } p.z = b.z + scater * tt.z; return p; } else if (onSegment(c, d, p)) { Point3 tt = d - c; Point3 ttpa = p - c; double scater = ttpa / tt; if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; } p.z = c.z + scater * tt.z; return p; } else if (onSegment(d, a, p)) { Point3 tt = a - d; Point3 ttpa = p - d; double scater = ttpa / tt; if (scater < 0 || scater>1) { return { -9999,-9999,-9999 }; } p.z = d.z + scater * tt.z; return p; } return { -9999,-9999,-9999 }; } else { w = sqrt(w); float ik2 = 0.5 / k2; float v = (-k1 - w) * ik2; float u = (h.x - f.x * v) / (e.x + g.x * v); if (u < 0.0 || u>1.0 || v < 0.0 || v>1.0) { v = (-k1 + w) * ik2; u = (h.x - f.x * v) / (e.x + g.x * v); } p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v; return p; } } p.z = a.z + (b.z - a.z) * u + (d.z - a.z) * v + (a.z - b.z + c.z - d.z) * u * v; return p; } Point3 BASECONSTVARIABLEAPI InverseDistanceWeighting(Point3 sreachp, QList ps) { // 反距离加权插值(IDW) // sreachp: 需要插值的点(x, y),z值将被插值 // ps: 已知点集,每个点包含x, y, z if (ps.isEmpty()) { sreachp.z = 0; return sreachp; } if (ps.size() == 1) { sreachp.z = ps[0].z; return sreachp; } double numerator = 0.0; double denominator = 0.0; double power = 2.0; // 常用幂次为2 for (const Point3& p : ps) { double dx = sreachp.x - p.x; double dy = sreachp.y - p.y; double dist = std::sqrt(dx * dx + dy * dy); // 距离为0,直接返回该点的z值 if (dist < 1e-12) { sreachp.z = p.z; return sreachp; } double weight = 1.0 / std::pow(dist, power); numerator += weight * p.z; denominator += weight; } if (denominator == 0.0) { sreachp.z = 0; } else { sreachp.z = numerator / denominator; } return sreachp; } double sind(double degree) { return sin(degree * d2r); } double cosd(double d) { return cos(d * d2r); } std::string Convert(float Num) { std::ostringstream oss; oss << Num; std::string str(oss.str()); return str; } QString JoinPath(const QString& path, const QString& filename) { QDir dir(path); // Ensure the path ends with the appropriate separator if (!QDir::isAbsolutePath(path)) dir.makeAbsolute(); return dir.filePath(filename); } std::vector convertQStringListToStdVector(const QStringList& qStringList) { std::vector stdVector; for (const QString& str : qStringList) { stdVector.push_back(str); } return stdVector; }; ErrorCode polyfit(const double* x, const double* y, int xyLength, int poly_n, std::vector& out_factor, double& out_chisq) { /* * x:自变量,视差 * y:因变量,距离 * xyLength: x、y长度 * poly_n:拟合的阶次 * out_factor:拟合的系数结果,从0阶到poly_n阶的系数 * out_chisq:拟合曲线与数据点的优值函数最小值 ,χ2 检验 */ gsl_matrix* XX = gsl_matrix_alloc(xyLength, poly_n + 1); gsl_vector* c = gsl_vector_alloc(poly_n + 1); gsl_matrix* cov = gsl_matrix_alloc(poly_n + 1, poly_n + 1); gsl_vector* vY = gsl_vector_alloc(xyLength); for (size_t i = 0; i < xyLength; i++) { gsl_matrix_set(XX, i, 0, 1.0); gsl_vector_set(vY, i, y[i]); for (int j = 1; j <= poly_n; j++) { gsl_matrix_set(XX, i, j, pow(x[i], j)); } } gsl_multifit_linear_workspace* workspace = gsl_multifit_linear_alloc(xyLength, poly_n + 1); int r = gsl_multifit_linear(XX, vY, c, cov, &out_chisq, workspace); gsl_multifit_linear_free(workspace); out_factor.resize(c->size, 0); for (size_t i = 0; i < c->size; ++i) { out_factor[i] = gsl_vector_get(c, i); } gsl_vector_free(vY); gsl_matrix_free(XX); gsl_matrix_free(cov); gsl_vector_free(c); return GSLState2ErrorCode(r); } Point3 crossProduct(const Point3& a, const Point3& b) { return Point3{ a.y * b.z - a.z * b.y, // C_x a.z * b.x - a.x * b.z, // C_y a.x * b.y - a.y * b.x // C_z }; } // 计算绕任意轴旋转的旋转矩阵 Eigen::Matrix3d rotationMatrix(const Eigen::Vector3d& axis, double angle) { // 确保旋转轴是单位向量 Eigen::Vector3d u = axis.normalized(); double cos_theta = cos(angle); double sin_theta = sin(angle); // 计算旋转矩阵 Eigen::Matrix3d R; R << cos_theta + u.x() * u.x() * (1 - cos_theta), u.x()* u.y()* (1 - cos_theta) - u.z() * sin_theta, u.x()* u.z()* (1 - cos_theta) + u.y() * sin_theta, u.y()* u.x()* (1 - cos_theta) + u.z() * sin_theta, cos_theta + u.y() * u.y() * (1 - cos_theta), u.y()* u.z()* (1 - cos_theta) - u.x() * sin_theta, u.z()* u.x()* (1 - cos_theta) - u.y() * sin_theta, u.z()* u.y()* (1 - cos_theta) + u.x() * sin_theta, cos_theta + u.z() * u.z() * (1 - cos_theta); return R; } long double convertToMilliseconds(const std::string& dateTimeStr) { // 定义日期时间字符串 std::string dateTimeString = dateTimeStr; // 使用 Boost.Date_Time 解析日期时间字符串 boost::posix_time::ptime dateTime = boost::posix_time::time_from_string(dateTimeString); // 将 ptime 转换为自 epoch (1970年1月1日) 以来的毫秒数 boost::posix_time::ptime epoch(boost::gregorian::date(1970, 1, 1)); boost::posix_time::time_duration duration = dateTime - epoch; long double milliseconds = duration.total_milliseconds() / 1000.0; return milliseconds; } long FindValueInStdVector(std::vector& list, double& findv) { if (list.size() == 0) { return -1; } else if (list.size() == 1) { if (abs(list[0] - findv) < 1e-9) { return 0; } else { return -1; } } else {} if (abs(list[0] - findv) < 1e-9) { return 0; } else if (abs(list[list.size() - 1] - findv) < 1e-9) { return list.size() - 1; } else if (list[0] > findv) { return -1; } else if (list[list.size() - 1] < findv) { return -1; } else {} long low = 0; long high = list.size() - 1; while (low <= high) { long mid = (low + high) / 2; if (abs(list[mid] - findv) < 1e-9) { return mid; } else if (list[mid] < findv) { low = mid + 1; } else if (list[mid] > findv) { high = mid - 1; } else {} } return -1; } long InsertValueInStdVector(std::vector& list, double insertValue, bool repeatValueInsert) { if (list.size() == 0) { list.insert(list.begin(), insertValue); return 0; } else if (list.size() == 1) { if (std::abs(list[0] - insertValue) < PRECISIONTOLERANCE) { if (repeatValueInsert) { list.push_back(insertValue); return 1; } else { return -1; } } else if (insertValue > list[0]) { list.push_back(insertValue); return 1; } else if (insertValue < list[0]) { list.push_back(insertValue); return 0; } } else { long low = 0; long high = list.size() - 1; long mid = -1; // 处理边界 if (list[high] <= insertValue) { if (std::abs(list[high] - insertValue) < PRECISIONTOLERANCE) { if (repeatValueInsert) { list.push_back(insertValue); } else {} } else { list.push_back(insertValue); } return list.size() - 1; } if (list[low] >= insertValue) { if (std::abs(list[low] - insertValue) < PRECISIONTOLERANCE) { if (repeatValueInsert) { list.insert(list.begin(), insertValue); } else {} } else { list.insert(list.begin(), insertValue); } return 0; } // low 1) { mid = (low + high) / 2; if (std::abs(list[mid] - insertValue) < PRECISIONTOLERANCE) { if (repeatValueInsert) { list.insert(list.begin() + mid, insertValue); } return mid; } else if (insertValue < list[mid]) { high = mid; } else if (list[mid] < insertValue) { low = mid; } } if (list[low] <= insertValue && insertValue <= list[high]) { if (std::abs(list[high] - insertValue) < PRECISIONTOLERANCE) { if (repeatValueInsert) { list.insert(list.begin() + high, insertValue); } else {} } else { list.insert(list.begin() + high, insertValue); } return low; } else { return -1; } } return -1; } long FindValueInStdVectorLast(std::vector& list, double& insertValue) { if (list.size() == 0 || list.size() == 1) { return -1; } else { long low = 0; long high = list.size() - 1; long mid = -1; // 处理边界 if (list[high]+ PRECISIONTOLERANCE < insertValue) { return -1; } else if (std::abs(list[high] - insertValue) < PRECISIONTOLERANCE) { return high; } else if (list[low] > insertValue) { return -1; } else if (std::abs(list[low] - insertValue) < PRECISIONTOLERANCE) { return low; } else {} // low 1) { mid = (low + high) / 2; if (insertValue < list[mid]) { high = mid; } else if (list[mid] < insertValue) { low = mid; } else {// insertValue==list[mid] , list[mid]===insertValue return mid;// } } if (list[low] <= insertValue && insertValue <= list[high]) { return low; } else { return -1; } } return -1; } ErrorCode polynomial_fit( const std::vector& x, const std::vector& y, int degree, std::vector& out_factor, double& out_chisq) { int xyLength = x.size(); double* xdata = new double[xyLength]; double* ydata = new double[xyLength]; for (long i = 0; i < xyLength; i++) { xdata[i] = x[i]; ydata[i] = y[i]; } ErrorCode state = polyfit(xdata, ydata, xyLength, degree, out_factor, out_chisq); delete[] xdata; delete[] ydata; // 释放内存 return state; } QVector SatellitePos2SatelliteAntPos(QVector poses) { QVector antposes(poses.count()); for (long i = 0; i < poses.count(); i++) { antposes[i].time = poses[i].time; antposes[i].Px = poses[i].Px; antposes[i].Py = poses[i].Py; antposes[i].Pz = poses[i].Pz; antposes[i].Vx = poses[i].Vx; antposes[i].Vy = poses[i].Vy; antposes[i].Vz = poses[i].Vz; } return antposes; } QVector SatelliteAntPos2SatellitePos(QVector poses) { QVector antposes(poses.count()); for (long i = 0; i < poses.count(); i++) { antposes[i].time = poses[i].time; antposes[i].Px = poses[i].Px; antposes[i].Py = poses[i].Py; antposes[i].Pz = poses[i].Pz; antposes[i].Vx = poses[i].Vx; antposes[i].Vy = poses[i].Vy; antposes[i].Vz = poses[i].Vz; } return antposes; } QString getDebugDataPath(QString filename) { QString folderName = "debugdata"; QString appDir = QCoreApplication::applicationDirPath(); QString folderpath = JoinPath(appDir, folderName); if (!QDir(folderpath).exists()) { QDir(appDir).mkdir(folderName); } QString datapath = JoinPath(folderpath, filename); QFile datafile(datapath); if (datafile.exists()) { datafile.remove(); } return datapath; } std::vector split(const std::string& str, char delimiter) { std::vector tokens; std::string token; std::istringstream tokenStream(str); while (std::getline(tokenStream, token, delimiter)) { tokens.push_back(token); } return tokens; } Eigen::VectorXd linspace(double start, double stop, int num) { Eigen::VectorXd result(num); double step = (stop - start) / (num - 1); // 计算步长 for (int i = 0; i < num; ++i) { result[i] = start + i * step; // 生成等间距数值 } return result; } void initializeMatrixWithSSE2(Eigen::MatrixXd& mat, const double* data, long rowcount, long colcount) { __m128d zero = _mm_setzero_pd(); #pragma omp parallel for for (long i = 0; i < rowcount; ++i) { for (long j = 0; j < colcount; j += 2) { // 每次处理2个double if (j + 2 <= colcount) { __m128d src = _mm_loadu_pd(&data[i * colcount + j]); _mm_storeu_pd(&mat(i, j), src); } else { // 处理剩余部分 for (long k = j; k < colcount; ++k) { mat(i, k) = data[i * colcount + k]; } } } } } void initializeMatrixWithSSE2(Eigen::MatrixXf& mat, const float* data, long rowcount, long colcount) { __m128 zero = _mm_setzero_ps(); #pragma omp parallel for for (long i = 0; i < rowcount; ++i) { for (long j = 0; j < colcount; j += 4) { // 每次处理4个float if (j + 4 <= colcount) { __m128 src = _mm_loadu_ps(&data[i * colcount + j]); _mm_storeu_ps(&mat(i, j), src); } else { // 处理剩余部分 for (long k = j; k < colcount; ++k) { mat(i, k) = data[i * colcount + k]; } } } } } Eigen::MatrixXd BASECONSTVARIABLEAPI MuhlemanSigmaArray(Eigen::MatrixXd& eta_deg) { Eigen::MatrixXd sigma = Eigen::MatrixXd::Zero(eta_deg.rows(), eta_deg.cols()); for (long i = 0; i < sigma.rows(); i++) { for (long j = 0; j < sigma.cols(); j++) { sigma(i,j) = calculate_MuhlemanSigma(eta_deg(i, j)); } } return sigma; } Eigen::MatrixXd BASECONSTVARIABLEAPI dB2Amp(Eigen::MatrixXd& sigma0) { Eigen::MatrixXd sigma = Eigen::MatrixXd::Zero(sigma0.rows(), sigma0.cols()); for (long i = 0; i < sigma.rows(); i++) { for (long j = 0; j < sigma.cols(); j++) { sigma(i, j) =std::pow(10.0, sigma0(i,j)/20.0); } } return sigma; } QDateTime parseCustomDateTime(const QString& dateTimeStr) { // 手动解析日期时间字符串 int year = dateTimeStr.left(4).toInt(); int month = dateTimeStr.mid(5, 2).toInt(); int day = dateTimeStr.mid(8, 2).toInt(); int hour = dateTimeStr.mid(11, 2).toInt(); int minute = dateTimeStr.mid(14, 2).toInt(); int second = dateTimeStr.mid(17, 2).toInt(); int msec = dateTimeStr.mid(20, 6).toInt(); // 只取毫秒的前三位,因为QDateTime支持到毫秒 // 创建 QDate 和 QTime 对象 QDate date(year, month, day); QTime time(hour, minute, second, msec ); // 转换为微秒,但QTime只支持毫秒精度 // 构造 QDateTime 对象 QDateTime result(date, time); return result; } bool isLeapYear(int year) { return (year % 4 == 0 && (year % 100 != 0 || year % 400 == 0)); } int daysInMonth(int year, int month) { if (month == 2) return isLeapYear(year) ? 29 : 28; else if (month == 4 || month == 6 || month == 9 || month == 11) return 30; else return 31; } TimestampMicroseconds parseAndConvert( std::string dateTimeStr) { // 解析年、月、日、时、分、秒和微秒 int year = std::stoi(dateTimeStr.substr(0, 4)); int month = std::stoi(dateTimeStr.substr(5, 2)); int day = std::stoi(dateTimeStr.substr(8, 2)); int hour = std::stoi(dateTimeStr.substr(11, 2)); int minute = std::stoi(dateTimeStr.substr(14, 2)); int second = std::stoi(dateTimeStr.substr(17, 2)); int microsec = std::stoi(dateTimeStr.substr(20, 6)); // 计算从1970年至目标年份前一年的总天数 long long totalDays = 0; for (int y = 1970; y < year; ++y) { totalDays += isLeapYear(y) ? 366 : 365; } // 加上目标年份从1月到上一个月的天数 for (int m = 1; m < month; ++m) { totalDays += daysInMonth(year, m); } // 加上本月的天数 totalDays += day - 1; // 转换为总秒数,再加上小时、分钟、秒 long long totalSeconds = totalDays * 24 * 60 * 60 + hour * 60 * 60 + minute * 60 + second; // 转换为毫秒和微秒 long long msecsSinceEpoch = totalSeconds * 1000 + microsec / 1000; int microseconds = microsec % 1000; return { msecsSinceEpoch, microseconds }; } bool BASECONSTVARIABLEAPI convertBitsPerSample(const char* bitsPerSampleVal, int32_t& result) { char* endPtr; errno = 0; // 重置错误标志 long longVal = std::strtol(bitsPerSampleVal, &endPtr, 10); // 十进制转换 // 检查转换错误 if (bitsPerSampleVal == endPtr) { return false; // 无有效字符 } if (errno == ERANGE || longVal < INT32_MIN || longVal > INT32_MAX) { return false; // 数值溢出 } if (*endPtr != '\0') { return false; // 含非数字字符 } result = static_cast(longVal); return true; // 成功 }