/******************** (C) COPYRIGHT 2020 Geek************************************ * File Name : pdr_util.c * Department : Sensor Algorithm Team * Current Version : V1.2 * Author : logzhan * Date of Issued : 2020.7.4 * Comments : PDR 工具类函数支持 ********************************************************************************/ /* Header File Including -----------------------------------------------------*/ #include #include #include #include "PDRBase.h" #include "Utils.h" #include "CoordTrans.h" const float FLT_THRES = 0.000001f; // 浮点数最小阈值 /* Function Declaration ------------------------------------------------------*/ /**---------------------------------------------------------------------- * Function : mean * Description : 对输入double数组求均值 * Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用 * 不同函数 * Date : 2020/7/4 logzhan *---------------------------------------------------------------------**/ double mean(double *data, int len) { int i; double value = 0; for (i = 0; i < len; i++) { value += data[i] / len; } return value; } /**---------------------------------------------------------------------- * Function : fmean * Description : 对输入float数组求均值 * Review : 求均值过程每次都做除法，有优化空间，可以针对求解均值的数值大小采用 * 不同函数 * Date : 2020/7/4 logzhan *---------------------------------------------------------------------**/ float fmean(float *data, int len) { float value = 0; for (int i = 0; i < len; i++){ value += data[i] / len; } return value; } double meanAngle(double* angle, int len) { double baseAngle = angle[0]; double diff = 0.0; double sum = 0.0; double value = 0; if (len <= 0)return 0; for (int i = 0; i < len; i++) { diff = angle[i] - baseAngle; if (diff > PI) { diff = diff - TWO_PI; } else if (diff < -PI) { diff = diff + TWO_PI; } sum += (baseAngle + diff); } value = sum / len; modAngle(&value, 0, TWO_PI); return value; } void modAngle(double * angle, double min, double max) { double value = *angle; double range = max - min; while (value > max)value = value - range; while (value < min)value = value + range; *angle = value; } float stdf(float* data, int len) { int i; float meanValue = fmean(data, len); float value = 0; for (i = 0; i < len; i++) { value += (data[i] - meanValue) * (data[i] - meanValue) / len; } return (float)sqrt(value); } /**---------------------------------------------------------------------- * Function : pdr_invSqrt * Description : 计算1/sqrt(x)的快速的算法 * Date : 2020/6/30 logzhan *---------------------------------------------------------------------**/ float InvSqrt(float x) { float xHalf = 0.5f * x; int i = *(int *)&x; i = 0x5f3759df - (i >> 1); x = *(float *)&i; x = x * (1.5f - xHalf * x * x); return x; } /**--------------------------------------------------------------------- * Function : Vec3Norm * Description : 三维浮点数向量归一化 * Date : 2022/11/1 logzhan *---------------------------------------------------------------------**/ void Vec3Norm(float* vx, float* vy, float* vz) { float norm = sqrtf(((*vx) * (*vx) + (*vy) * (*vy) + (*vz) * (*vz))); // 防止出现模为0的情况 if (norm > FLT_THRES) { norm = 1 / norm; (*vx) *= norm; (*vy) *= norm; (*vz) *= norm; } } /**---------------------------------------------------------------------- * Function : Motion2TypeStr * Description : 把用户运动类型转换为字符串输出 * Date : 2022/9/16 logzhan *---------------------------------------------------------------------**/ const char* Motion2TypeStr(int type) { if (type == PDR_MOTION_TYPE_STATIC) { return "STATIC"; } else if (type == PDR_MOTION_TYPE_IRREGULAR) { return "IRREGULAR"; } else if (type == PDR_MOTION_TYPE_HANDHELD) { return "HANDLED"; } else if (type == PDR_MOTION_TYPE_SWINGING) { return "SWINGING"; } return "UNKOWN"; } /**---------------------------------------------------------------------- * Function : qnb2att * Description : 四元数转欧拉角 * Date : 2020/7/4 logzhan *---------------------------------------------------------------------**/ void Qnb2Att(float* q, double* attitude) { double q11 = q[0] * q[0]; double q13 = q[0] * q[2]; double q12 = q[0] * q[1]; double q14 = q[0] * q[3]; double q22 = q[1] * q[1]; double q24 = q[1] * q[3]; double q23 = q[1] * q[2]; double q33 = q[2] * q[2]; double q34 = q[2] * q[3]; double q44 = q[3] * q[3]; if (fabs(q34 + q12) <= 0.5) { attitude[0] = asin(2 * (q34 + q12)); } attitude[1] = atan2(-2 * (q24 - q13), q11 - q22 - q33 + q44); attitude[2] = atan2(-2 * (q23 - q14), q11 - q22 + q33 - q44); } void GetCovMatrix(double *x, double *y, double cov[2][2], int n) { int i; double sum_x = 0, sum_y = 0, sum_xy = 0; double m_x = 0, m_y = 0; m_x = mean(x, n); m_y = mean(y, n); for (i = 0; i < n; i++) { sum_xy += (x[i] - m_x)*(y[i] - m_y); sum_x += (x[i] - m_x)*(x[i] - m_x); sum_y += (y[i] - m_y)*(y[i] - m_y); } cov[0][0] = sum_x / n; cov[0][1] = sum_xy / n; cov[1][0] = sum_xy / n; cov[1][1] = sum_y / n; } int Jacobi(double a[][2], double p[][2], int n, double eps, int T) { int i, j, k; double max = a[0][1]; int row = 0; int col = 0; int ite_num = 0; double theta = 0; double aii = 0; double ajj = 0; double aij = 0; double sin_theta = 0; double cos_theta = 0; double sin_2theta = 0; double cos_2theta = 0; double arowk = 0; double acolk = 0; double pki = 0; double pkj = 0; for (i = 0; i < n; i++) p[i][i] = 1; while (1) { max = fabs(a[0][1]); row = 0; col = 1; for (i = 0; i < n; i++) for (j = 0; j < n; j++) if (i != j && fabs(a[i][j])>max) { max = fabs(a[i][j]); row = i; col = j; } if (max < eps) { //printf("max : %lf \n", max); //printf("t : %lf \n", ite_num); return 1; } if (ite_num>T) { //printf("max : %lf \n", max); //printf("t : %lf \n", ite_num); return 0; } if (a[row][row] == a[col][col]) theta = PI / 4; else theta = 0.5*atan(2 * a[row][col] / (a[row][row] - a[col][col])); aii = a[row][row]; ajj = a[col][col]; aij = a[row][col]; sin_theta = sin(theta); cos_theta = cos(theta); sin_2theta = sin(2 * theta); cos_2theta = cos(2 * theta); a[row][row] = aii * cos_theta*cos_theta + ajj * sin_theta*sin_theta + aij * sin_2theta; a[col][col] = aii * sin_theta*sin_theta + ajj * cos_theta*cos_theta - aij * sin_2theta; a[row][col] = 0.5*(ajj - aii)*sin_2theta + aij * cos_2theta; a[col][row] = a[row][col]; for (k = 0; k < n; k++) { if (k != row && k != col) { arowk = a[row][k]; acolk = a[col][k]; a[row][k] = arowk * cos_theta + acolk * sin_theta; a[k][row] = a[row][k]; a[col][k] = acolk * cos_theta - arowk * sin_theta; a[k][col] = a[col][k]; } } if (ite_num == 0) { p[row][row] = cos_theta; p[row][col] = -sin_theta; p[col][row] = sin_theta; p[col][col] = cos_theta; } else { for (k = 0; k < n; k++) { pki = p[k][row]; pkj = p[k][col]; p[k][row] = pki * cos_theta + pkj * sin_theta; p[k][col] = pkj * cos_theta - pki * sin_theta; } } ite_num++; } } static int isRealLat(double lat){ return (lat < 90) && (lat > -90); } static int isRealLon(double lon){ return (lon < 180) && (lon > -180); } /* function: 计算地图上两个由经纬度表示点间的直线距离。 param @ pointA : 点A的经纬度 param @ pointB : 点B的经纬度 return @ dis : A、B点间的距离；若小于0则说明A或B点的经纬度输入有误。 */ double CalDistance(LatLng_t pointA, LatLng_t pointB) { double dis = -1; double lla1[3] = { 0 }; double lla2[3] = { 0 }; double ned1[3] = { 0 }; double ned2[3] = { 0 }; double diff_ned[3] = { 0 }; lla1[0] = pointA.lat * RADIAN_PER_DEG; lla1[1] = pointA.lon * RADIAN_PER_DEG; lla2[0] = pointB.lat * RADIAN_PER_DEG; lla2[1] = pointB.lon * RADIAN_PER_DEG; if (isRealLat(pointA.lat) && isRealLon(pointA.lon) && isRealLat(pointB.lat) && isRealLon(pointB.lon)) { WGS842NED(lla1, lla1, ned1); WGS842NED(lla2, lla1, ned2); diff_ned[0] = ned1[0] - ned2[0]; diff_ned[1] = ned1[1] - ned2[1]; diff_ned[2] = ned1[2] - ned2[2]; dis = sqrt(pow(diff_ned[0], 2) + pow(diff_ned[1], 2) + pow(diff_ned[2], 2)); } return dis; } LatLng_t ProjPointOfLatLng(LatLng_t point, LatLng_t linePointA, LatLng_t linePointB) { /* local variables declaration */ double gradxB_P; /* linePointB 向 point 的 x 梯度 */ double gradxB_A; /* linePointA 向 linePointB 的 x 梯度 */ double gradyB_A; /* linePointA 向 linePointB 的 y 梯度 */ double normPow; /* linePointA 向 linePointB 梯度的平方和 */ double scalarMultip; /* gradxB_A * gradyB_A 的标量乘法 */ LatLng_t resProjPoint; /* 结果点坐标 */ /* block process */ gradxB_A = linePointA.lat - linePointB.lat; gradyB_A = linePointA.lon - linePointB.lon; if ((fabs(gradxB_A) < DBL_EPSILON) && (fabs(gradyB_A) < DBL_EPSILON)) { /* 当linePointA 和linePointB是同一点时，返回的point坐标 */ resProjPoint.lat = point.lat; resProjPoint.lon = point.lon; } else { gradxB_P = point.lat - linePointB.lat; normPow = pow(gradxB_A, 2) + pow(gradyB_A, 2); resProjPoint.lon = (double)( (gradxB_P * gradxB_A * gradyB_A + point.lon * pow(gradyB_A, 2) + linePointB.lon * pow(gradxB_A, 2)) / normPow); if ((fabs(gradxB_A) < DBL_EPSILON) || (fabs(gradyB_A) < DBL_EPSILON)) { /* 当linePointA 和 linePointB 所在直线与 x轴或y轴平行时做规避*/ scalarMultip = (fabs(gradxB_A) < DBL_EPSILON) ? gradyB_A : gradxB_A; } else { scalarMultip = gradxB_A * gradyB_A; } resProjPoint.lat = (double)( (gradxB_A * linePointB.lat * gradyB_A + gradxB_A * gradxB_A * (resProjPoint.lon - linePointB.lon)) / scalarMultip); if (fabs(gradxB_A) < DBL_EPSILON) { resProjPoint.lat = linePointA.lat; } if (fabs(gradyB_A) < DBL_EPSILON) { resProjPoint.lon = point.lon; } } return resProjPoint; } /**---------------------------------------------------------------------- * Function : CalEarthParameter * Description : 根据输入纬度返回该纬度地球相关参数 * Date : 2020/7/8 logzhan *---------------------------------------------------------------------**/ EarthData_t CalEarthParameter(double lat) { double temp_value = 0; EarthData_t earthData = { 0 }; if (fabs(lat) >= PI / 2)lat = 0.0f; temp_value = sqrt(1 - WGS84_ECCENTR2 * (sin(lat)*sin(lat))); earthData.lat = lat; earthData.rmh = WGS84_RE * (1 - WGS84_ECCENTR2) / (temp_value*temp_value*temp_value); earthData.rnh = WGS84_RE * cos(lat) / temp_value; return earthData; } /**---------------------------------------------------------------------- * Function : pdr_CalRadianDifferent * Description : 计算弧度之差 * Date : 2020/7/8 logzhan *---------------------------------------------------------------------**/ double CalRadianDifferent(double s_dir, double d_dir) { double dirDiff = d_dir - s_dir; if (fabs(dirDiff) < PI)return dirDiff; return dirDiff > 0 ? dirDiff - 2 * PI : dirDiff + 2 * PI; } void ProjPointOfLatLng_cir(double point1[], double yaw, double point2[], double result[]) { if (fabs(fabs(yaw) - PI / 2) < DOUBLE_ZERO) { result[0] = point1[0]; result[1] = point2[1]; return; } EarthData_t earthData = CalEarthParameter(point1[0]); double k = -tan(yaw); double lonDiff = (point2[1] - point1[1]) * earthData.rnh; double latDiff = (point2[0] - point1[0]) * earthData.rmh; double x = (k*latDiff + k * k * lonDiff) / (1 + k * k); double y = (latDiff + k * lonDiff) / (1 + k * k); result[1] = point1[1] + x / earthData.rnh; result[0] = point1[0] + y / earthData.rmh; } /**---------------------------------------------------------------------- * Function : pdr_WriteCsvStr * Description : 将字符串信息写入文本，主要是对fprintf封装一层 * Date : 2020/7/8 logzhan *---------------------------------------------------------------------**/ void WriteCsvStr(FILE* fp_write, char* str) { if (NULL == fp_write)return; fprintf(fp_write, "%s\n", str); } int pdr_min(int a, int b){ return (a > b ? b : a); } /**---------------------------------------------------------------------- * Function : pdr_utc2hms * Description : 将UTC时间转换为时分秒 * Date : 2020/7/8 logzhan *---------------------------------------------------------------------**/ void pdr_utc2hms(double utc, double* h, double* m, double* s) { long lutc = (long)(utc / 1000); lutc = lutc % (3600 * 24); *h = floor((double)(lutc / (3600))); *m = floor((lutc - (*h) * 3600) / 60); *s = lutc - (*h) * 3600 - (*m) * 60; }