|
|
|
|
|
#include "Arith_Utils.h"
|
|
|
|
|
|
#include <cmath>
|
|
|
|
|
|
|
|
|
|
|
|
#include "Arith_SysStruct.h"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
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);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
#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
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
unsigned char FourPointInterpolation(unsigned char* pSrcImage, int iWidth, int iHeight, float x, float y)
|
|
|
|
|
|
{
|
|
|
|
|
|
if (x < 0 || y < 0 || x > iWidth || y > iHeight)
|
|
|
|
|
|
return 0;
|
|
|
|
|
|
// 四个最临近象素的坐标(i1, j1), (i2, j1), (i1, j2), (i2, j2)
|
|
|
|
|
|
int i1, i2;
|
|
|
|
|
|
int j1, j2;
|
|
|
|
|
|
|
|
|
|
|
|
// 四个最临近象素值
|
|
|
|
|
|
unsigned char f1, f2, f3, f4;
|
|
|
|
|
|
|
|
|
|
|
|
// 计算四个最临近象素的坐标
|
|
|
|
|
|
i1 = fmax((int)x, 0);
|
|
|
|
|
|
i2 = fmin(i1 + 1, iWidth - 1);
|
|
|
|
|
|
j1 = fmax((int)y, 0);
|
|
|
|
|
|
j2 = fmin(j1 + 1, iHeight - 1);
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
float t, s; //t = x - [x], s = y- [y];
|
|
|
|
|
|
|
|
|
|
|
|
t = x - i1;
|
|
|
|
|
|
s = y - j1;
|
|
|
|
|
|
|
|
|
|
|
|
//四个最临近象素点的值的加权值
|
|
|
|
|
|
float uv0, uv1, uv2, uv3; //权重之和为1：uv0+uv1+uv2+uv3 = 1
|
|
|
|
|
|
uv0 = (1 - s) * (1 - t);
|
|
|
|
|
|
uv1 = (1 - s) * t;
|
|
|
|
|
|
uv2 = (1 - t) * s;
|
|
|
|
|
|
uv3 = s * t;
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//根据不同情况分别处理
|
|
|
|
|
|
if (x - iWidth + 1 > 0)
|
|
|
|
|
|
{
|
|
|
|
|
|
// 要计算的点在图像右边缘上
|
|
|
|
|
|
if (y - iHeight + 1 < 0)
|
|
|
|
|
|
{
|
|
|
|
|
|
// 要计算的点正好是图像最右/左下角那一个象素，直接返回该点象素值
|
|
|
|
|
|
f1 = *(pSrcImage + iWidth * j1 + i1);
|
|
|
|
|
|
return f1;
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
// 在图像右/左边缘上且不是最后一点，直接一次插值即可
|
|
|
|
|
|
f1 = *(pSrcImage + iWidth * j1 + i1);
|
|
|
|
|
|
f3 = *(pSrcImage + iWidth * j2 + i1);
|
|
|
|
|
|
|
|
|
|
|
|
// 返回插值结果
|
|
|
|
|
|
return ((unsigned char)(f1 + s * (f3 - f1)));
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
|
|
else if (y - iHeight + 1 > 0)
|
|
|
|
|
|
{
|
|
|
|
|
|
// 要计算的点在图像下边缘上且不是最后一点，直接一次插值即可
|
|
|
|
|
|
f1 = *(pSrcImage + iWidth * j1 + i1);
|
|
|
|
|
|
f2 = *(pSrcImage + iWidth * j1 + i2);
|
|
|
|
|
|
|
|
|
|
|
|
// 返回插值结果
|
|
|
|
|
|
return ((unsigned char)(f1 + t * (f2 - f1)));
|
|
|
|
|
|
}
|
|
|
|
|
|
else
|
|
|
|
|
|
{
|
|
|
|
|
|
// 计算四个最临近象素值
|
|
|
|
|
|
f1 = *(pSrcImage + iWidth * j1 + i1);
|
|
|
|
|
|
f2 = *(pSrcImage + iWidth * j1 + i2);
|
|
|
|
|
|
f3 = *(pSrcImage + iWidth * j2 + i1);
|
|
|
|
|
|
f4 = *(pSrcImage + iWidth * j2 + i2);
|
|
|
|
|
|
|
|
|
|
|
|
return (unsigned char)(uv0 * f1 + uv1 * f2 + uv2 * f3 + uv3 * f4);
|
|
|
|
|
|
}
|
|
|
|
|
|
}
|
