void TForm2::testEyeFit() {
double monitorWidth = 326.0; //mm
double monitorHeight = 260.0; //mm
double cameraWidth = 352; //pixels
double cameraHeight = 240; //pixels
int nPoints = 0;
/* FILE *fp = fopen("C:\\Users\\mgrivich\\data.csv", "r");
while(fgets(str,sizeof(str),fp) != NULL) {
i++;
}
*/
// rewind(fp);
std::ifstream ifs;
ifs.open ( "C:\\Users\\mgrivich\\data.csv"); // declare file stream: http://www.cplusplus.com/reference/iostream/ifstream/
string value;
while (ifs.good()) {
getline(ifs, value);
nPoints++;
}
nPoints--;
double **coords = new2D(nPoints, 2, 0.0);
double **data = new2D(nPoints, 2, 0.0);
double **error = new2D(nPoints,2,0.0);
ifs.clear();
ifs.seekg(0);
for(int i=0; i<nPoints; i++) {
getline(ifs, value, ',');
coords[i][0] = monitorWidth*((1000.0 - atof(value.c_str()))/1000.0);
getline(ifs, value, ',');
coords[i][1] = monitorHeight*((1000.0 - atof(value.c_str()))/1000.0);
getline(ifs, value, ',');
data[i][0] = atof(value.c_str())-cameraWidth/2.0;
getline(ifs, value, ',');
data[i][1] = cameraHeight/2.0 - atof(value.c_str());
getline(ifs, value, ',');
error[i][0] = atof(value.c_str());
getline(ifs, value);
error[i][1] = atof(value.c_str());
}
ifs.close();
/* for(int k =0; k<nPoints; k++) {
printf( "%g,%g,%g,%g,%g,%g\n", coords[k][0], coords[k][1], measured[k][0], measured[k][1], error[k][0], error[k][0]);
} */
// double params[11] = {10.0, 570.0, 130.0, 185.0, -.4836443450476, -1.1132326618289, .8172934386507,
// -2.1796283945831, 8.0098006074300, 33.3526742746832, 563.0};
double params[11] = {10.0, 570.0, 130.0, 185.0, 0, 0, 0,
0, 0, 50, 563.0};
double *spread = new1D<double>(11, 0.0);
// spread[0] = 3;
// spread[1] = 50;
// spread[2] = 50;
// spread[3] = 50;
spread[4] = 3.14/2;
spread[5] = 3.14/2;
spread[6] = 3.14/2;
spread[7] = 5;
spread[8] = 5;
spread[9] = 30;
spread[10] = 0;
EyeDeltaFunction *df = new EyeDeltaFunction(coords, data, error, nPoints, 2, params, 11);
df->setStartingSpread(spread);
Minimizer *m = new Minimizer(df);
m->DoMinimize();
double* pOut = df->getParameters();
printf("R: %g\n", pOut[0]);
printf("x0: %g\n", pOut[1]);
printf("y0: %g\n", pOut[2]);
printf("z0: %g\n", pOut[3]);
printf("a: %g\n", pOut[4]);
printf("b: %g\n", pOut[5]);
printf("g: %g\n", pOut[6]);
printf("bx: %g\n", pOut[7]);
printf("by: %g\n", pOut[8]);
printf("dc: %g\n", pOut[9]);
printf("z: %g\n", pOut[10]);
spread[1] = 50;
spread[2] = 50;
spread[3] = 50;
df->setStartingSpread(spread);
m->DoMinimize();
pOut = df->getParameters();
printf("R: %g\n", pOut[0]);
printf("x0: %g\n", pOut[1]);
printf("y0: %g\n", pOut[2]);
printf("z0: %g\n", pOut[3]);
printf("a: %g\n", pOut[4]);
printf("b: %g\n", pOut[5]);
printf("g: %g\n", pOut[6]);
printf("bx: %g\n", pOut[7]);
printf("by: %g\n", pOut[8]);
printf("dc: %g\n", pOut[9]);
printf("z: %g\n", pOut[10]);
spread[0] = 3;
// spread[10] = 20;
df->setStartingSpread(spread);
m->DoMinimize();
pOut = df->getParameters();
printf("R: %g\n", pOut[0]);
printf("x0: %g\n", pOut[1]);
printf("y0: %g\n", pOut[2]);
printf("z0: %g\n", pOut[3]);
printf("a: %g\n", pOut[4]);
printf("b: %g\n", pOut[5]);
printf("g: %g\n", pOut[6]);
printf("bx: %g\n", pOut[7]);
printf("by: %g\n", pOut[8]);
printf("dc: %g\n", pOut[9]);
printf("z: %g\n", pOut[10]);
}
TetrahedronMesh::TetrahedronMesh(const int size)
{
int trianglesPerFace = size*size;
numFaces = trianglesPerFace * 4;
vertices.resize(4 + 6 * (size - 1) + 2 * (size - 2)*(size - 1));
triangles.resize(4 * trianglesPerFace);
float3 initialPos = { size / 2.0, size / 2.0, size / 2.0 };
float3 deltaX = { 0, 1, -1 };
float3 deltaY = { -1, -1, 0 };
int** indices1 = new2D(size + 1);
int** indices2 = new2D(size + 1);
int** indices3 = new2D(size + 1);
int** indices4 = new2D(size + 1);
int coordCount = 0, triangleCount = 0;
// PLane 1:
for (int y = 0; y <= size; y++){
float3 currentPos = initialPos + deltaY * y;
for (int x = 0; x < y + 1; x++){
indices1[x][y] = coordCount;
vertices[coordCount++] = currentPos;
if (x > 0) {
triangles[triangleCount++] = { indices1[x][y], indices1[x - 1][y], indices1[x - 1][y - 1] };
if (x < y) {
triangles[triangleCount++] = { indices1[x][y], indices1[x - 1][y - 1], indices1[x][y - 1] };
}
}
currentPos += deltaX;
}
}
// Plane 2:
deltaX = { 1, 0, -1 };
for (int y = 0; y <= size; y++){
float3 currentPos = initialPos + deltaY * y;
for (int x = 0; x < y + 1; x++){
if (x == 0){
indices2[x][y] = indices1[x][y];
}
else{
indices2[x][y] = coordCount;
vertices[coordCount++] = currentPos;
}
if (x > 0) {
triangles[triangleCount++] = { indices2[x][y], indices2[x - 1][y - 1], indices2[x - 1][y] };
if (x < y) {
triangles[triangleCount++] = { indices2[x][y], indices2[x][y - 1], indices2[x - 1][y - 1] };
}
}
currentPos += deltaX;
}
}
// plane 3
initialPos = { -size / 2.0, size / 2.0, -size / 2.0 };
deltaY = { 1, -1, 0 };
deltaX = { 0, 1, 1 };
for (int y = 0; y <= size; y++){
float3 currentPos = initialPos + deltaY * y;
for (int x = 0; x < y + 1; x++){
if (y == size){
indices3[x][y] = indices2[size - x][size - x];
}
else if (x == y){
indices3[x][y] = indices1[size - x][size - x];
}
else{
indices3[x][y] = coordCount;
vertices[coordCount++] = currentPos;
}
if (x > 0) {
triangles[triangleCount++] = { indices3[x][y], indices3[x - 1][y], indices3[x - 1][y - 1] };
if (x < y) {
triangles[triangleCount++] = { indices3[x][y], indices3[x - 1][y - 1], indices3[x][y - 1] };
}
}
currentPos += deltaX;
}
}
//plane 4
deltaX = { -1, 0, 1 };
for (int y = 0; y <= size; y++){
float3 currentPos = initialPos + deltaY * y;
for (int x = 0; x < y + 1; x++){
if (y == size){
indices4[x][y] = indices2[size - x][size];
}
else if (x == y){
indices4[x][y] = indices1[size - y][size];
}
else if (x == 0){
indices4[x][y] = indices3[x][y];
}
else{
indices4[x][y] = coordCount;
vertices[coordCount++] = currentPos;
}
if (x > 0) {
triangles[triangleCount++] = { indices4[x][y], indices4[x - 1][y - 1], indices4[x - 1][y] };
if (x < y) {
triangles[triangleCount++] = { indices4[x][y], indices4[x][y - 1], indices4[x - 1][y - 1] };
}
}
currentPos += deltaX;
}
}
initCudaBuffers();
}
void TForm2::testRigidFit() {
int nPoints = 4;
double aIn = .3;
double bIn = .8;
double gIn = .7;
double xIn = 0;
double yIn = 0;
double zIn = 0;
double sa = sin(aIn);
double sb = sin(bIn);
double sg = sin(gIn);
double ca = cos(aIn);
double cb = cos(bIn);
double cg = cos(gIn);
//rotation matrix
double r00 = ca*cb;
double r01 = ca*sb*sg - sa*cg;
double r02 = ca*sb*cg + sa*sg;
double r10 = sa*cb;
double r11 = sa*sb*sg + ca*cg;
double r12 = sa*sb*cg - ca*sg;
double r20 = -sb;
double r21 = cb*sg;
double r22 = cb*cg;
double **orig = new2D(nPoints, 3, 0.0);
orig[0][0] = 0.0;
orig[0][1] = 0.0;
orig[0][2] = 0.0;
orig[1][0] = 1.0;
orig[1][1] = 0.0;
orig[1][2] = 0.0;
orig[2][0] = 0.0;
orig[2][1] = 1.0;
orig[2][2] = 0.0;
orig[3][0] = 0.0;
orig[3][1] = 0.0;
orig[3][2] = 1.0;
double **current = new2D(nPoints, 3, 0.0);
for(int i=0; i<nPoints; i++) {
double x = orig[i][0];
double y = orig[i][1];
double z = orig[i][2];
double tx = r00*x + r01*y + r02*z;
double ty = r10*x + r11*y + r12*z;
double tz = r20*x + r21*y + (r22)*z;
current[i][0] = tx + xIn;
current[i][1] = ty + yIn;
current[i][2] = tz + zIn;
printf("current[i][0]: %g\n", current[i][0]);
printf("current[i][1]: %g\n", current[i][1]);
printf("current[i][2]: %g\n\n", current[i][2]);
}
double **error = new2D(nPoints,3,1.0);
double params[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
double *spread = new1D<double>(6, 0.0);
spread[0] = 3.14/2.0;
spread[1] = 3.14/2.0;
spread[2] = 3.14/2.0;
spread[3] = 1.0;
spread[4] = 1.0;
spread[5] = 1.0;
RigidDeltaFunction *df = new RigidDeltaFunction(orig, current, error, nPoints, 3, params, 6);
df->setStartingSpread(spread);
Minimizer *m = new Minimizer(df);
m->nmax = 1000;
m->DoMinimize();
double* pOut = df->getParameters();
printf("a: %g\n", pOut[0]);
printf("b: %g\n", pOut[1]);
printf("g: %g\n", pOut[2]);
printf("x0: %g\n", pOut[3]);
printf("y0: %g\n", pOut[4]);
printf("z0: %g\n", pOut[5]);
delete2D(error, nPoints);
delete2D(current, nPoints);
delete2D(orig, nPoints);
delete1D(spread);
delete df;
}
