|
|
#include <cmath>
|
|
|
#include <iostream>
|
|
|
#include <fstream>
|
|
|
#include <string>
|
|
|
#include <cstring>
|
|
|
#include <cstdlib>
|
|
|
#include "Arith_Utils.h"
|
|
|
#include "Arith_SysStruct.h"
|
|
|
|
|
|
#ifdef __linux__
|
|
|
#include <unistd.h>
|
|
|
#define ACCESS access
|
|
|
#elif _WIN32
|
|
|
#include <windows.h>
|
|
|
#include <io.h>
|
|
|
#define ACCESS _access
|
|
|
#endif
|
|
|
|
|
|
|
|
|
void AddMat(double* A, double* B, int mn, double* AaddB)
|
|
|
{
|
|
|
for (size_t i = 0; i < mn; i++)
|
|
|
{
|
|
|
AaddB[i] = A[i] + B[i];
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
void MultiMatScalar(double scalar, double* A, double* sA, int mn)
|
|
|
{
|
|
|
for (size_t i = 0; i < mn; i++)
|
|
|
{
|
|
|
sA[i] = scalar * A[i];
|
|
|
}
|
|
|
}
|
|
|
|
|
|
void TransposeMat(double* A, int w, int h, double* AT)
|
|
|
{
|
|
|
for (int i = 0; i < h; i++)
|
|
|
{
|
|
|
for (int j = 0; j < w; j++)
|
|
|
{
|
|
|
AT[j * w + i] = A[i * h + j];
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
|
|
|
/* 1 0 0
|
|
|
0 cos sin
|
|
|
0 -sin cos
|
|
|
*/
|
|
|
void Rx(double omega, double* Opp)
|
|
|
{
|
|
|
double coso = cos(RADIAN(omega));
|
|
|
double sino = sin(RADIAN(omega));
|
|
|
|
|
|
Opp[0] = 1;
|
|
|
Opp[1] = 0;
|
|
|
Opp[2] = 0;
|
|
|
Opp[3] = 0;
|
|
|
Opp[4] = coso;
|
|
|
Opp[5] = sino;
|
|
|
Opp[6] = 0;
|
|
|
Opp[7] = -sino;
|
|
|
Opp[8] = coso;
|
|
|
}
|
|
|
|
|
|
/* cos 0 sin
|
|
|
0 1 0
|
|
|
-sin 0 cos
|
|
|
*/
|
|
|
void Ry(double omega, double* Opp)
|
|
|
{
|
|
|
double coso = cos(RADIAN(omega));
|
|
|
double sino = sin(RADIAN(omega));
|
|
|
|
|
|
Opp[0] = coso;
|
|
|
Opp[1] = 0;
|
|
|
Opp[2] = sino;
|
|
|
Opp[3] = 0;
|
|
|
Opp[4] = 1;
|
|
|
Opp[5] = 0;
|
|
|
Opp[6] = -sino;
|
|
|
Opp[7] = 0;
|
|
|
Opp[8] = coso;
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
/* cos sin 0
|
|
|
-sin cos 0
|
|
|
0 0 1
|
|
|
*/
|
|
|
void Rz(double omega, double* Opp)
|
|
|
{
|
|
|
double coso = cos(RADIAN(omega));
|
|
|
double sino = sin(RADIAN(omega));
|
|
|
|
|
|
Opp[0] = coso;
|
|
|
Opp[1] = sino;
|
|
|
Opp[2] = 0;
|
|
|
Opp[3] = -sino;
|
|
|
Opp[4] = coso;
|
|
|
Opp[5] = 0;
|
|
|
Opp[6] = 0;
|
|
|
Opp[7] = 0;
|
|
|
Opp[8] = 1;
|
|
|
}
|
|
|
|
|
|
// 镜面反射矩阵
|
|
|
void RMirror(PointXYZ normalVec, double* Opp)
|
|
|
{
|
|
|
// M = eye(3)-(2*N*N');
|
|
|
Opp[0] = 1 - 2 * normalVec.X * normalVec.X;
|
|
|
Opp[1] = -2 * normalVec.X * normalVec.Y;
|
|
|
Opp[2] = -2 * normalVec.X * normalVec.Z;
|
|
|
Opp[3] = -2 * normalVec.X * normalVec.Y;
|
|
|
Opp[4] = 1 - 2 * normalVec.Y * normalVec.Y;
|
|
|
Opp[5] = -2 * normalVec.Y * normalVec.Z;
|
|
|
Opp[6] = -2 * normalVec.X * normalVec.Z;
|
|
|
Opp[7] = -2 * normalVec.Y * normalVec.Z;
|
|
|
Opp[8] = 1 - 2 * normalVec.Z * normalVec.Z;
|
|
|
|
|
|
}
|
|
|
//*********************************************************************************************
|
|
|
// 矩阵乘法
|
|
|
//*********************************************************************************************
|
|
|
void MultiMat(double* A, double* B, double* AB, int m, int n, int p)
|
|
|
{
|
|
|
// A：m*n ; B：n*p ;
|
|
|
int i, j, k;
|
|
|
double Multisum;
|
|
|
|
|
|
for (i = 0; i < m; i++)
|
|
|
{
|
|
|
for (j = 0; j < p; j++)
|
|
|
{
|
|
|
Multisum = 0;
|
|
|
for (k = 0; k < n; k++)
|
|
|
Multisum += A[i * n + k] * B[k * p + j];
|
|
|
|
|
|
AB[i * p + j] = Multisum;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
//*********************************************************************************************
|
|
|
//
|
|
|
//*********************************************************************************************
|
|
|
void MultiMat33(double* A, double* B, double* AB)
|
|
|
{
|
|
|
MultiMat(A, B, AB, 3, 3, 3);
|
|
|
}
|
|
|
|
|
|
//*********************************************************************************************
|
|
|
//
|
|
|
//*********************************************************************************************
|
|
|
void MultiMat333(double* A, double* B, double* C, double* ABC)
|
|
|
{
|
|
|
double BC[9];
|
|
|
MultiMat(B, C, BC, 3, 3, 3);
|
|
|
MultiMat(A, BC, ABC, 3, 3, 3);
|
|
|
}
|
|
|
|
|
|
// 对点/矢量旋转
|
|
|
PointXYZ RtPoint(double* M, PointXYZ Point)
|
|
|
{
|
|
|
// M * [X,Y,Z]'
|
|
|
double A[3];
|
|
|
double G[3];
|
|
|
A[0] = Point.X;
|
|
|
A[1] = Point.Y;
|
|
|
A[2] = Point.Z;
|
|
|
MultiMat(M, A, G, 3, 3, 1);
|
|
|
|
|
|
PointXYZ result;
|
|
|
result.X = G[0];
|
|
|
result.Y = G[1];
|
|
|
result.Z = G[2];
|
|
|
return result;
|
|
|
|
|
|
}
|
|
|
|
|
|
// 标准化矢量
|
|
|
PointXYZ NormPoint(PointXYZ Point)
|
|
|
{
|
|
|
double Len = sqrt(Point.X * Point.X + Point.Y * Point.Y + Point.Z * Point.Z);
|
|
|
Point.X /= Len;
|
|
|
Point.Y /= Len;
|
|
|
Point.Z /= Len;
|
|
|
return Point;
|
|
|
}
|
|
|
|
|
|
|
|
|
void eye3(double* M)
|
|
|
{
|
|
|
memset(M, 0, sizeof(double) * 9);
|
|
|
M[0] = 1;
|
|
|
M[4] = 1;
|
|
|
M[8] = 1;
|
|
|
}
|
|
|
|
|
|
void invMat3(double* A, double* invA)
|
|
|
{
|
|
|
double a1 = A[0];
|
|
|
double a2 = A[1];
|
|
|
double a3 = A[2];
|
|
|
double b1 = A[3];
|
|
|
double b2 = A[4];
|
|
|
double b3 = A[5];
|
|
|
double c1 = A[6];
|
|
|
double c2 = A[7];
|
|
|
double c3 = A[8];
|
|
|
|
|
|
//A*
|
|
|
double Astar[9] = { 0 };
|
|
|
Astar[0] = b2 * c3 - c2 * b3;
|
|
|
Astar[1] = c1 * b3 - b1 * c3;
|
|
|
Astar[2] = b1 * c2 - c1 * b2;
|
|
|
|
|
|
Astar[3] = c2 * a3 - a2 * c3;
|
|
|
Astar[4] = a1 * c3 - c1 * a3;
|
|
|
Astar[5] = a2 * c1 - a1 * c2;
|
|
|
|
|
|
Astar[6] = a2 * b3 - b2 * a3;
|
|
|
Astar[7] = b1 * a3 - a1 * b3;
|
|
|
Astar[8] = a1 * b2 - a2 * b1;
|
|
|
|
|
|
double detA = a1 * (b2 * c3 - c2 * b3) - a2 * (b1 * c3 - c1 * b3) + a3 * (b1 * c2 - c1 * b2);
|
|
|
|
|
|
for (size_t i = 0; i < 9; i++)
|
|
|
{
|
|
|
invA[i] = Astar[i] / detA;
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
// 旋转向量转矩阵
|
|
|
void rotationVectorToMatrix(PointXYZ rotaVec_One, float rotaAgl, double* Mat)
|
|
|
{
|
|
|
//
|
|
|
PointXYZ vec_norm = NormPoint(rotaVec_One);
|
|
|
|
|
|
// K
|
|
|
double K[9] = { 0,-vec_norm.Z,vec_norm.Y,
|
|
|
vec_norm.Z,0,-vec_norm.X,
|
|
|
-vec_norm.Y,vec_norm.X,0 };
|
|
|
|
|
|
// eye(3)
|
|
|
double eye[9] = { 0 };
|
|
|
eye3(eye);
|
|
|
|
|
|
// eye + sind(r) * K + (1-cos(r)) * K * K
|
|
|
double sK[9] = { 0 };
|
|
|
MultiMatScalar(sin(RADIAN(rotaAgl)), K, sK, 9);
|
|
|
|
|
|
double KK[9] = { 0 };
|
|
|
MultiMat33(K, K, KK);
|
|
|
|
|
|
double scK[9] = { 0 };
|
|
|
MultiMatScalar(1 - cos(RADIAN(rotaAgl)), KK, scK, 9);
|
|
|
|
|
|
double tmp[9] = { 0 };
|
|
|
AddMat(eye, sK, 9, tmp);
|
|
|
|
|
|
AddMat(tmp, scK, 9, Mat);
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
// 初始化哈希表
|
|
|
void initHashMap(HashMap* map, int capacity)
|
|
|
{
|
|
|
map->entries = (HashEntry*)malloc(capacity * sizeof(HashEntry));
|
|
|
if (NULL == map->entries)
|
|
|
{
|
|
|
return;
|
|
|
}
|
|
|
map->capacity = capacity;
|
|
|
map->size = 0;
|
|
|
for (int i = 0; i < capacity; i++)
|
|
|
{
|
|
|
map->entries[i].key = 0;
|
|
|
map->entries[i].count = 0;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 查找哈希表中的键
|
|
|
int findKey(HashMap* map, int key)
|
|
|
{
|
|
|
for (int i = 0; i < map->size; i++)
|
|
|
{
|
|
|
if (map->entries[i].key == key)
|
|
|
{
|
|
|
return i;
|
|
|
}
|
|
|
}
|
|
|
return -1;
|
|
|
}
|
|
|
|
|
|
// 插入键值对到哈希表
|
|
|
void insert(HashMap* map, int key)
|
|
|
{
|
|
|
int index = findKey(map, key);
|
|
|
if (index == -1)
|
|
|
{
|
|
|
if (map->size < map->capacity)
|
|
|
{
|
|
|
map->entries[map->size].key = key;
|
|
|
map->entries[map->size].count = 1;
|
|
|
map->size++;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
printf("Hash map is full.\n");
|
|
|
}
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
map->entries[index].count++;
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 释放哈希表
|
|
|
void freeHashMap(HashMap* map)
|
|
|
{
|
|
|
free(map->entries);
|
|
|
}
|
|
|
|
|
|
|
|
|
int findMostFrequentNonZero(int arr[], int size, int *clsMaxNum, float *MostFrequentratio)
|
|
|
{
|
|
|
HashMap map;
|
|
|
initHashMap(&map, size); // 假设数组中的数字范围较小
|
|
|
|
|
|
// 记录每个非0数字出现的次数
|
|
|
for (int i = 0; i < size; i++)
|
|
|
{
|
|
|
if (arr[i] != 0)
|
|
|
{
|
|
|
insert(&map, arr[i]);
|
|
|
}
|
|
|
}
|
|
|
|
|
|
// 找到出现次数最多的数字
|
|
|
int mostFrequentNum = 0;
|
|
|
int maxFrequency = 0;
|
|
|
int totalNum = 0;
|
|
|
for (int i = 0; i < map.size; i++)
|
|
|
{
|
|
|
if (map.entries[i].count > maxFrequency)
|
|
|
{
|
|
|
mostFrequentNum = map.entries[i].key;
|
|
|
maxFrequency = map.entries[i].count;
|
|
|
}
|
|
|
if (map.entries[i].count > 0)
|
|
|
{
|
|
|
totalNum += map.entries[i].count;
|
|
|
}
|
|
|
}
|
|
|
*clsMaxNum = maxFrequency;
|
|
|
*MostFrequentratio = maxFrequency / (totalNum + FEPSILON);
|
|
|
|
|
|
freeHashMap(&map);
|
|
|
return mostFrequentNum;
|
|
|
}
|
|
|
|
|
|
int count2To0Transitions(int arr[], int size)
|
|
|
{
|
|
|
int count = 0;
|
|
|
for (int i = 0; i < size - 1; i++)
|
|
|
{
|
|
|
if (arr[i] == 2 && arr[i + 1] == 0)
|
|
|
{
|
|
|
count++;
|
|
|
}
|
|
|
}
|
|
|
return count;
|
|
|
}
|
|
|
|
|
|
bool checkXToYCondition(int arr[], int size, int x, int y, int frmThresh)
|
|
|
{
|
|
|
bool foundTransition = false;
|
|
|
int consecutiveOnes = 0;
|
|
|
|
|
|
for (int i = 0; i < size - 1; i++)
|
|
|
{
|
|
|
if (arr[i] == x && arr[i + 1] == y)
|
|
|
{
|
|
|
foundTransition = true;
|
|
|
consecutiveOnes = 1; // 从 1 开始计数
|
|
|
i++; // 跳过已经检查的 1
|
|
|
|
|
|
// 继续检查后续的 1
|
|
|
while (i < size - 1 && arr[i] == y)
|
|
|
{
|
|
|
consecutiveOnes++;
|
|
|
i++;
|
|
|
}
|
|
|
|
|
|
// 检查连续 1 的次数是否大于 阈值
|
|
|
if (consecutiveOnes > frmThresh)
|
|
|
{
|
|
|
return true;
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
return false;
|
|
|
}
|
|
|
|
|
|
int countNumberFreq(int arr[], int size, int number)
|
|
|
{
|
|
|
int count = 0;
|
|
|
for (int i = 0; i < size; i++)
|
|
|
{
|
|
|
if (arr[i] == 2)
|
|
|
{
|
|
|
count++;
|
|
|
}
|
|
|
}
|
|
|
return count;
|
|
|
}
|
|
|
|
|
|
static float get_dot_prod(float* A, float* B, int size)
|
|
|
{
|
|
|
int i;
|
|
|
float dot_prod = 0.f;
|
|
|
for (i = 0; i < size; i++)
|
|
|
{
|
|
|
dot_prod += A[i] * B[i];
|
|
|
}
|
|
|
|
|
|
return dot_prod;
|
|
|
}
|
|
|
|
|
|
static float get_eucl_magn(float* A, float* B, int size)
|
|
|
{
|
|
|
int i;
|
|
|
double A_len = 0;
|
|
|
double B_len = 0;
|
|
|
float eucl_magn = 0;
|
|
|
for (i = 0; i < size; i++)
|
|
|
{
|
|
|
A_len += pow(A[i], 2);
|
|
|
B_len += pow(B[i], 2);
|
|
|
}
|
|
|
eucl_magn = (float)sqrt(A_len * B_len);
|
|
|
return eucl_magn;
|
|
|
}
|
|
|
|
|
|
float Cosine_Similarity(float* A, float* B, int size)
|
|
|
{
|
|
|
// Vectors size can't be 0
|
|
|
if (size == 0)
|
|
|
{
|
|
|
//throw std::logic_error("Vector size can not be 0.");
|
|
|
return 0;
|
|
|
}
|
|
|
float value = 0;
|
|
|
float dot_prod = 0;
|
|
|
// 计算欧式距离
|
|
|
float eucl_magn = get_eucl_magn(A, B, size);
|
|
|
// If eucledian distance is 0 skip calculation
|
|
|
if (eucl_magn == 0)
|
|
|
{
|
|
|
value = 0;
|
|
|
}
|
|
|
else
|
|
|
{
|
|
|
dot_prod = get_dot_prod(A, B, size);
|
|
|
value = dot_prod / eucl_magn;
|
|
|
}
|
|
|
return value;
|
|
|
}
|
|
|
|
|
|
|
|
|
#ifdef _WIN32 // Windows平台
|
|
|
|
|
|
#include <Windows.h>
|
|
|
|
|
|
std::string GetDynamicLibraryPath()
|
|
|
{
|
|
|
HMODULE hModule = GetModuleHandle(NULL);
|
|
|
char path[MAX_PATH];
|
|
|
GetModuleFileName(hModule, path, MAX_PATH);
|
|
|
std::string fullPath(path);
|
|
|
size_t pos = fullPath.find_last_of("\\/");
|
|
|
return fullPath.substr(0, pos);
|
|
|
}
|
|
|
|
|
|
#elif __linux__ // Linux平台
|
|
|
|
|
|
|
|
|
#include <dlfcn.h>
|
|
|
#include <unistd.h>
|
|
|
|
|
|
std::string GetDynamicLibraryPath()
|
|
|
{
|
|
|
Dl_info dlInfo;
|
|
|
dladdr((void*)GetDynamicLibraryPath, &dlInfo);
|
|
|
char* path = realpath(dlInfo.dli_fname, NULL);
|
|
|
std::string fullPath(path);
|
|
|
free(path);
|
|
|
size_t pos = fullPath.find_last_of("/");
|
|
|
return fullPath.substr(0, pos);
|
|
|
}
|
|
|
|
|
|
#else
|
|
|
|
|
|
#error Unsupported platform
|
|
|
|
|
|
#endif
|
|
|
|
|
|
|
|
|
void Save_RunImg(GD_VIDEO_FRAME_S img, SINT32 nControlerFrmNum)
|
|
|
{
|
|
|
char resPath[256] = { 0 };
|
|
|
std::string path = GetDynamicLibraryPath();
|
|
|
snprintf(resPath, 256, "%s/%s", path.c_str(), "SaveImg");
|
|
|
if (ACCESS(resPath, 0) == 0)
|
|
|
{// SaveImg文件存在,写数据,默认在第10帧写数据
|
|
|
if (10 == nControlerFrmNum)
|
|
|
{
|
|
|
std::string filename = std::string(path) + "/frame_" + std::to_string(nControlerFrmNum);
|
|
|
std::ofstream outFile(filename, std::ios::binary);
|
|
|
if (!outFile)
|
|
|
{
|
|
|
std::cerr << "Failed to open file for writing: " << filename << std::endl;
|
|
|
}
|
|
|
|
|
|
switch (img.enPixelFormat)
|
|
|
{
|
|
|
case GD_PIXEL_FORMAT_GRAY_Y8:
|
|
|
for (unsigned int y = 0; y < img.u32Height; ++y)
|
|
|
{
|
|
|
outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[0] + y * img.u32Stride[0]), img.u32Width);
|
|
|
}
|
|
|
break;
|
|
|
case GD_PIXEL_FORMAT_GRAY_Y16:
|
|
|
for (unsigned int y = 0; y < img.u32Height; ++y)
|
|
|
{
|
|
|
outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[0] + y * img.u32Stride[0]), img.u32Width * 2);
|
|
|
}
|
|
|
break;
|
|
|
case GD_PIXEL_FORMAT_NV12:
|
|
|
// 写入Y平面
|
|
|
for (unsigned int y = 0; y < img.u32Height; ++y)
|
|
|
{
|
|
|
outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[0] + y * img.u32Stride[0]), img.u32Width);
|
|
|
}
|
|
|
// 写入UV平面
|
|
|
for (unsigned int y = 0; y < img.u32Height / 2; ++y)
|
|
|
{
|
|
|
outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[1] + y * img.u32Stride[1]), img.u32Stride[1]);
|
|
|
}
|
|
|
break;
|
|
|
case GD_PIXEL_FORMAT_RGB_PACKED:
|
|
|
for (unsigned int y = 0; y < img.u32Height; ++y)
|
|
|
{
|
|
|
outFile.write(reinterpret_cast<const char*>(img.u64VirAddr[0] + y * img.u32Stride[0]), img.u32Width * 3);
|
|
|
}
|
|
|
break;
|
|
|
default:
|
|
|
std::cerr << "Unsupported pixel format: " << img.enPixelFormat << std::endl;
|
|
|
outFile.close();
|
|
|
}
|
|
|
|
|
|
outFile.close();
|
|
|
}
|
|
|
}
|
|
|
}
|
|
|
|
|
|
|
|
|
FLOAT32 SERVO_CorrectAngleScale(FLOAT32 fAngle)
|
|
|
{
|
|
|
FLOAT32 fAngleCorrect = fAngle;
|
|
|
|
|
|
//若角度小于伺服角度下限（-180°），则其实际值已经跨入0°～180°范围的右侧
|
|
|
if (fAngle < SERVO_ANGLE_MIN)
|
|
|
fAngleCorrect = fAngle + SERVO_ANGLE_RANGE;
|
|
|
|
|
|
//若角度大于伺服角度上限（180°），则其实际值已经跨入-180°～0°的范围左侧
|
|
|
if (fAngle >= SERVO_ANGLE_MAX)
|
|
|
fAngleCorrect = fAngle - SERVO_ANGLE_RANGE;
|
|
|
|
|
|
return fAngleCorrect;
|
|
|
}
|
