543 lines
12 KiB
C++
543 lines
12 KiB
C++
#include "stdafx.h"
|
||
#define EIGEN_USE_BLAS
|
||
#define EIGEN_VECTORIZE_SSE4_2
|
||
//#include <mkl.h>
|
||
|
||
#include <iostream>
|
||
#include <Eigen/Core>
|
||
#include <Eigen/Dense>
|
||
#include <time.h>
|
||
////#include <mkl.h>
|
||
#include <string>
|
||
#include <omp.h>
|
||
#include <QDir>
|
||
#include < io.h >
|
||
#include < stdio.h >
|
||
#include < stdlib.h >
|
||
#include <gdal.h>
|
||
#include <gdal_priv.h>
|
||
#include <gdalwarper.h>
|
||
//#include <ogr_geos.h>
|
||
#include <ogrsf_frmts.h> //#include "ogrsf_frmts.h"
|
||
|
||
#include <fstream>
|
||
#include <proj.h>
|
||
#include "GeoOperator.h"
|
||
|
||
#include <iostream>
|
||
#include <boost/date_time/posix_time/posix_time.hpp>
|
||
#include "baseTool.h"
|
||
|
||
#include <gsl/gsl_fit.h> // 多项式拟合
|
||
#include <gsl/gsl_cdf.h> /* 提供了 gammaq 函数 */
|
||
#include <gsl/gsl_vector.h> /* 提供了向量结构*/
|
||
#include <gsl/gsl_matrix.h>
|
||
#include <gsl/gsl_multifit.h>
|
||
|
||
|
||
|
||
|
||
|
||
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<QString> splitString(const QString& str, char delimiter)
|
||
{
|
||
QStringList tokens = str.split(delimiter);
|
||
return convertQStringListToStdVector(tokens);
|
||
}
|
||
|
||
|
||
std::complex<double> Cubic_Convolution_interpolation(double u, double v, Eigen::MatrixX<std::complex<double>> img)
|
||
{
|
||
if (img.rows() != 4 || img.cols() != 4) {
|
||
throw std::exception("the size of img's block is not right");
|
||
}
|
||
// 斤拷锟斤拷模锟斤拷
|
||
Eigen::MatrixX<std::complex<double>> wrc(1, 4);// 使锟斤拷 complex<double> 斤拷锟斤拷要原斤拷为锟剿伙拷取值
|
||
Eigen::MatrixX<std::complex<double>> 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<std::complex<double>> interValue = wrc * img * wcr;
|
||
return interValue(0, 0);
|
||
}
|
||
|
||
std::complex<double> Cubic_kernel_weight(double s)
|
||
{
|
||
s = abs(s);
|
||
if (s <= 1) {
|
||
return std::complex<double>(1.5 * s * s * s - 2.5 * s * s + 1, 0);
|
||
}
|
||
else if (s <= 2) {
|
||
return std::complex<double>(-0.5 * s * s * s + 2.5 * s * s - 4 * s + 2, 0);
|
||
}
|
||
else {
|
||
return std::complex<double>(0, 0);
|
||
}
|
||
}
|
||
|
||
/// <summary>
|
||
/// p11 p12 -- x
|
||
/// p0(u,v)
|
||
/// p21 p22
|
||
/// |
|
||
/// y
|
||
/// p11(0,0)
|
||
/// p21(0,1)
|
||
/// P12(1,0)
|
||
/// p22(1,1)
|
||
/// </summary>
|
||
/// <param name="p0">x,y,z</param>
|
||
/// <param name="p1">x,y,z</param>
|
||
/// <param name="p2">x,y,z</param>
|
||
/// <param name="p3">x,y,z</param>
|
||
/// <param name="p4">x,y,z</param>
|
||
/// <returns></returns>
|
||
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;
|
||
}
|
||
|
||
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<QString> convertQStringListToStdVector(const QStringList& qStringList)
|
||
{
|
||
std::vector<QString> 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<double>& 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<double>& 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<double>& 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<insertValue < high
|
||
while ((high - 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<double>& 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] <= 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<insertValue < high
|
||
while ((high - 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;
|
||
}
|
||
|