383 lines
10 KiB
C++
383 lines
10 KiB
C++
///#include "stdafx.h"
|
|
#include "HeadTracker.h"
|
|
#include "POsit_3D.h"
|
|
|
|
u32 From_Scratch = 1;
|
|
float AverageE_2D = 0.0f;
|
|
|
|
#define ROTATION_FREEDOM 1
|
|
//#pragma optimize ("",off)
|
|
|
|
|
|
Vector TransformedPoints[MAX_POINTS];
|
|
Vector ProjectedPoints[MAX_POINTS];
|
|
Vector ScreenDirs[MAX_POINTS];
|
|
|
|
Vector Project(Vector &V, float focale)
|
|
{
|
|
Vector Ret;
|
|
float Ooz;
|
|
Ooz = 1.0f / V.z;
|
|
Ret.x = focale * V.x * Ooz;
|
|
Ret.y = focale * V.y * Ooz;
|
|
Ret.z = Ooz;
|
|
//Ret = V;
|
|
return Ret;
|
|
}
|
|
|
|
|
|
static void UpdateMatrix(MATRIX *MatrixDST, float CurrentFocale, Vector *DataPoints , Vector *ImagePoints , u32 ulNumberOfPoints )
|
|
{
|
|
Vector *TranDest,*TranDestLast,*ProjDest;
|
|
TranDest = TransformedPoints;
|
|
ProjDest = ProjectedPoints;
|
|
TranDestLast = TranDest+ulNumberOfPoints;
|
|
while (TranDest < TranDestLast)
|
|
{
|
|
TranDest->z = -1.0f;
|
|
if (*(u32 *)&ImagePoints->z != 0xC0DE2006)
|
|
{
|
|
*TranDest = *MatrixDST * *DataPoints;
|
|
if (TranDest->z > 0.0f)
|
|
{
|
|
Vector Ret;
|
|
float Ooz;
|
|
Ooz = 1.0f / TranDest->z;
|
|
ProjDest->x = CurrentFocale * TranDest->x * Ooz;
|
|
ProjDest->y = CurrentFocale * TranDest->y * Ooz;
|
|
ProjDest->z = Ooz;
|
|
}
|
|
}
|
|
ImagePoints++;
|
|
TranDest++;
|
|
DataPoints++;
|
|
ProjDest++;
|
|
}
|
|
}
|
|
|
|
|
|
float Compute_Matrix_E(MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints ,u32 LasKeyIndex)
|
|
{
|
|
float fRealNumber = 0.0f;
|
|
float Norm1 = 0.0f;
|
|
for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
|
|
{
|
|
if (TransformedPoints[Counter2].z > 0.0f)
|
|
{
|
|
Vector Local;
|
|
Local = ImagePoints[Counter2] - ProjectedPoints[Counter2];
|
|
Norm1 += Local.NormXY();
|
|
fRealNumber++;
|
|
}
|
|
};
|
|
if (fRealNumber) Norm1 /= fRealNumber;
|
|
return Norm1;
|
|
}
|
|
|
|
static float Compute_Matrix_T(MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints ,u32 Mode3D)
|
|
{
|
|
Vector Projected;
|
|
Vector Centroide_DP,Centroide_IP;
|
|
Vector Centroide_DPX,TRANSLATOR,T;
|
|
float fRealNumber,Norm_DP,Norm_IP;
|
|
float Oofoc = 1.0f/CurrentFocale;
|
|
Centroide_DPX = Centroide_DP = Centroide_IP = Vector(0,0,0);
|
|
fRealNumber = 0.0f;
|
|
TRANSLATOR = Vector(0,0,0);
|
|
for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
|
|
{
|
|
if (*(u32 *)&ImagePoints[Counter2].z != 0xC0DE2006)
|
|
{
|
|
T = *MatrixDST * DataPoints[Counter2];
|
|
if (T.z > 0.0f)
|
|
{
|
|
TRANSLATOR += (ImagePoints[Counter2] * (T.z * Oofoc));
|
|
TRANSLATOR -= T;
|
|
fRealNumber ++;
|
|
}
|
|
}
|
|
};
|
|
if (fRealNumber)
|
|
{
|
|
Centroide_IP.z = Centroide_DP.z = 0.0f;
|
|
fRealNumber = 1.0f/fRealNumber;
|
|
TRANSLATOR *= fRealNumber ;
|
|
TRANSLATOR.z = 0.0f;
|
|
MatrixDST->T += TRANSLATOR;
|
|
if (Mode3D == 1)
|
|
{
|
|
MatrixDST->K = MatrixDST->T ;
|
|
MatrixDST->K.Normalize();
|
|
MatrixDST->I = MatrixDST->J^MatrixDST->K;
|
|
MatrixDST->I.Normalize();
|
|
MatrixDST->J = MatrixDST->K^MatrixDST->I;
|
|
MatrixDST->J.Normalize();
|
|
}
|
|
|
|
UpdateMatrix(MatrixDST, CurrentFocale, DataPoints , ImagePoints , ulNumberOfPoints );
|
|
fRealNumber = 0.0f;
|
|
for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
|
|
{
|
|
if (TransformedPoints[Counter2].z > 0.0f)
|
|
{
|
|
Centroide_DPX += TransformedPoints[Counter2];
|
|
Centroide_DP += ProjectedPoints[Counter2];
|
|
Centroide_IP += ImagePoints[Counter2];
|
|
fRealNumber++;
|
|
}
|
|
};
|
|
Centroide_IP.z = Centroide_DP.z = 0.0f;
|
|
fRealNumber = 1.0f / fRealNumber;
|
|
Centroide_IP *= fRealNumber;
|
|
Centroide_DP *= fRealNumber;
|
|
Centroide_DPX*= fRealNumber;
|
|
Norm_DP = Norm_IP = 0.0f;
|
|
for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
|
|
{
|
|
if (TransformedPoints[Counter2].z > 0.0f)
|
|
{
|
|
Norm_DP += (ProjectedPoints[Counter2] - Centroide_DP).NormXY();
|
|
Norm_IP += (ImagePoints[Counter2] - Centroide_IP).NormXY();
|
|
}
|
|
};
|
|
if (Norm_IP && Norm_DP)
|
|
{
|
|
MatrixDST->T -= Centroide_DPX;
|
|
Centroide_DPX *= (Norm_DP / Norm_IP);
|
|
MatrixDST->T += Centroide_DPX;//*/
|
|
UpdateMatrix(MatrixDST, CurrentFocale, DataPoints , ImagePoints , ulNumberOfPoints );
|
|
}
|
|
}
|
|
|
|
return Compute_Matrix_E(MatrixDST, CurrentFocale, ImagePoints , DataPoints , ulNumberOfPoints ,0);
|
|
}
|
|
|
|
static void Compute_Matrix(u32 Dim,MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints , u32 LasKeyIndex,u32 Mode3D)
|
|
{
|
|
Vector Local;
|
|
Vector Projected;
|
|
float Norm1,Alpha,Angle;
|
|
MATRIX MatrixRes;
|
|
|
|
MatrixRes = *MatrixDST;
|
|
|
|
Norm1 = Compute_Matrix_T(&MatrixRes, CurrentFocale, ImagePoints , DataPoints , ulNumberOfPoints ,Dim);
|
|
for (u32 Counter = 0; Counter < 1 ; Counter ++)
|
|
{
|
|
/* 2 : Rotation */
|
|
u32 Zou;
|
|
#define MAX_CONVERG 7
|
|
|
|
Alpha = 3.1415927f*2.0f ;Angle = 0;Zou=0;
|
|
if (!From_Scratch)
|
|
{
|
|
while (Zou < MAX_CONVERG - 5)
|
|
{
|
|
Alpha*=0.25f;
|
|
Zou++;
|
|
}
|
|
}
|
|
for (; Zou < MAX_CONVERG ; Zou++,Alpha *= 0.25f)
|
|
{
|
|
float Norm[6],NormMin;
|
|
MATRIX MatrixRot[6];
|
|
u32 GoodI;
|
|
NormMin = Norm1;
|
|
for (int g = 0 ; g < 6; g ++)
|
|
{
|
|
MatrixRot[g] = MatrixRes;
|
|
if (Dim == 1)
|
|
MatrixRot[g].RotateAround(0,g & 1 ? + Alpha : - Alpha);
|
|
else
|
|
MatrixRot[g].RotateAround(g>>1,g & 1 ? + Alpha : - Alpha);
|
|
Norm[g] = Compute_Matrix_T(&MatrixRot[g], CurrentFocale, ImagePoints , DataPoints , ulNumberOfPoints ,Dim);
|
|
if (NormMin > Norm[g])
|
|
{
|
|
GoodI = g;
|
|
NormMin = Norm[g];
|
|
}
|
|
}
|
|
if (NormMin < Norm1)
|
|
{
|
|
Norm1 = NormMin;
|
|
MatrixRes = MatrixRot[GoodI];
|
|
}
|
|
};
|
|
};
|
|
*MatrixDST = MatrixRes;
|
|
}
|
|
static void Estimate_Z(MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 Reset)
|
|
{
|
|
Vector TEST,Gerr,Projected,CameraInObjectSpace;
|
|
MATRIX MatrixDST_I;
|
|
if (Reset) return;
|
|
MatrixDST->Inverse(MatrixDST_I);
|
|
CameraInObjectSpace = Vector(0,0,0);
|
|
CameraInObjectSpace = MatrixDST_I * CameraInObjectSpace;
|
|
MatrixDST_I.T = Vector(0,0,0);
|
|
{
|
|
for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
|
|
{
|
|
if (*(u32 *)&ImagePoints[Counter2].z != 0xC0DE2006)
|
|
{
|
|
Vector Transformed,Transformed2,Local2,Local;
|
|
Vector V1,V2,V3,V4,V5;
|
|
float NV1;
|
|
V1 = DataPoints[Counter2] - CameraPos[Counter2];
|
|
NV1 = V1.Norm();
|
|
if (NV1 > 0.00001f)
|
|
{
|
|
float Angle;
|
|
Transformed = ImagePoints[Counter2];
|
|
Transformed.z = CurrentFocale;
|
|
Transformed.Normalize();
|
|
V2 = MatrixDST_I * Transformed;
|
|
V1.Normalize();
|
|
V2.Normalize();
|
|
Angle = V1 * V2;
|
|
if (Angle < 0.95f)
|
|
{
|
|
float Error;
|
|
V3 = V1 ^ V2;
|
|
V3.Normalize();
|
|
V5 = CameraInObjectSpace - DataPoints[Counter2];
|
|
Error = (V5 * V3) / NV1;
|
|
Error *= Error;
|
|
if (Error < 1.0f/ 100000.0f)
|
|
{
|
|
V4 = V2 ^ V3;
|
|
V4.Normalize();
|
|
V1 *= 1.0f / (V1 * V4);
|
|
V1 *= (V5 * V4);
|
|
DataPoints[Counter2] += V1;
|
|
//CameraPos[Counter2] = DataPoints[Counter2];
|
|
}
|
|
}
|
|
}
|
|
}
|
|
};
|
|
};
|
|
}
|
|
|
|
|
|
static u32 Decimate(MATRIX *MatrixDST, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , u32 ulNumberOfPoints , u32 SizeX )
|
|
{
|
|
Vector Error,Gerr,Projected,L;
|
|
float AverageE;
|
|
u32 ReturnValue;
|
|
AverageE = 0;
|
|
ReturnValue = 0;
|
|
|
|
for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
|
|
{
|
|
if (*(u32 *)&ImagePoints[Counter2].z != 0xC0DE2006)
|
|
{
|
|
L = *MatrixDST * DataPoints[Counter2];
|
|
if (L .z > 0.0f)
|
|
{
|
|
ReturnValue++;
|
|
Projected = Project(L, CurrentFocale);
|
|
Gerr = ImagePoints[Counter2] - Projected;
|
|
AverageE += (Gerr.x * Gerr.x + Gerr.y * Gerr.y);
|
|
}
|
|
}
|
|
};
|
|
if (ReturnValue)
|
|
{
|
|
AverageE *= ((float)1.0f / (float)ReturnValue);
|
|
#define E_MIN (12.0f / 320.0f)
|
|
#define E_MAX (25.0f / 320.0f)
|
|
if (AverageE < E_MIN* E_MIN) AverageE = E_MIN* E_MIN;//*/
|
|
//if (AverageE > E_MAX* E_MAX) AverageE = E_MAX* E_MAX;//*/
|
|
ReturnValue = 0;
|
|
for (u32 Counter2 = 0; Counter2 < ulNumberOfPoints ; Counter2 ++)
|
|
{
|
|
float LocalEr;
|
|
L = *MatrixDST * DataPoints[Counter2];
|
|
if (L.z > 0.0f)
|
|
{
|
|
Projected = Project(L, CurrentFocale);
|
|
Error = ImagePoints[Counter2] - Projected;
|
|
LocalEr = (Error.x * Error.x + Error.y * Error.y);
|
|
if (LocalEr > AverageE)
|
|
{
|
|
*(u32 *)&ImagePoints[Counter2].z = 0xC0DE2006;
|
|
} else
|
|
{
|
|
ImagePoints[Counter2].z = 1.0f;
|
|
ReturnValue++;
|
|
}
|
|
}
|
|
else
|
|
*(u32 *)&ImagePoints[Counter2].z = 0xC0DE2006;
|
|
};
|
|
};
|
|
return ReturnValue;
|
|
}
|
|
|
|
|
|
u32 POsit(u32 _2D_3D, MATRIX *MatrixSrc, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 LasKeyIndex , u32 SizeX, u32 SizeY)
|
|
{
|
|
u32 Ret;
|
|
MATRIX MatrixSrc2;
|
|
static u32 Countttt;
|
|
MatrixSrc2 = *MatrixSrc;
|
|
|
|
Countttt = ulNumberOfPoints;
|
|
while (Countttt--) ImagePoints[Countttt].z = 1.0f;
|
|
SizeX>>=1;
|
|
u32 Conv = 3;
|
|
//From_Scratch = 1;
|
|
if (From_Scratch)
|
|
{
|
|
MatrixSrc->Identity();
|
|
if (_2D_3D == 1)
|
|
MatrixSrc->T.z = 50.0f;
|
|
else
|
|
MatrixSrc->T.z = 50.0f;
|
|
MatrixSrc2 = *MatrixSrc;
|
|
Conv = 10;
|
|
Compute_Matrix(_2D_3D,MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints ,LasKeyIndex, 1);
|
|
}
|
|
|
|
Compute_Matrix_T(MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints ,_2D_3D);
|
|
|
|
while (Conv--)
|
|
{
|
|
Ret = Decimate(MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints,SizeX);
|
|
if (!From_Scratch) *MatrixSrc = MatrixSrc2;
|
|
Compute_Matrix(_2D_3D,MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints ,LasKeyIndex, 1);
|
|
};
|
|
|
|
Ret = Decimate(MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , ulNumberOfPoints,SizeX);
|
|
MatrixSrc->I.Normalize();
|
|
MatrixSrc->J.Normalize();
|
|
MatrixSrc->K.Normalize();
|
|
|
|
|
|
if (Ret < 8)
|
|
From_Scratch = 1;
|
|
else
|
|
{
|
|
From_Scratch = 0;
|
|
if (Ret > ulNumberOfPoints - (ulNumberOfPoints >> 3))
|
|
{
|
|
if (CameraPos)
|
|
Estimate_Z(MatrixSrc, CurrentFocale, (Vector *)ImagePoints , (Vector *)DataPoints , (Vector *)CameraPos , ulNumberOfPoints ,0);
|
|
}
|
|
}
|
|
return Ret;
|
|
}
|
|
|
|
|
|
u32 POsit_3D(MATRIX *MatrixSrc, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 LasKeyIndex , u32 SizeX, u32 SizeY)
|
|
{
|
|
return POsit(3,MatrixSrc,CurrentFocale,ImagePoints , DataPoints , CameraPos , ulNumberOfPoints , LasKeyIndex , SizeX, SizeY);
|
|
|
|
}
|
|
|
|
u32 POsit_2D(MATRIX *MatrixSrc, float CurrentFocale, Vector *ImagePoints , Vector *DataPoints , Vector *CameraPos , u32 ulNumberOfPoints , u32 LasKeyIndex , u32 SizeX, u32 SizeY)
|
|
{
|
|
return POsit(1,MatrixSrc,CurrentFocale,ImagePoints , DataPoints , CameraPos , ulNumberOfPoints , LasKeyIndex , SizeX, SizeY);
|
|
}
|
|
|