private double[] findClosestTranslation(
Plane3D[] elements_scene,
Plane3D[] model,
Plane3D[] modeltr,
int[] closests,
double variance) {
double[] ret = new double[2];
double total;
double[] distances;
double[][] traslations;
Plane3D element_scene;
Plane3D element_model;
Plane3D element_modeltr;
double angle;
double dist;
double total_dist;
double less_total;
int len, len2;
int count, count2;
len = elements_scene.length;
len2 = model.length;
distances = new double[len2];
traslations = new double[2][len2];
Point3D projection;
total = 0;
for (count = 0; count < len2; count++) {
element_modeltr = modeltr[count];
less_total = Double.MAX_VALUE;
for (count2 = 0; count2 < len; count2++) {
element_scene = elements_scene[count2];
angle =
element_modeltr.vector.getX() * element_scene.vector.getX()
+ element_modeltr.vector.getZ() * element_scene.vector.getZ();
angle /=
Math.sqrt(
element_modeltr.vector.getX() * element_modeltr.vector.getX()
+ element_modeltr.vector.getZ() * element_modeltr.vector.getZ())
* Math.sqrt(
element_scene.vector.getX() * element_scene.vector.getX()
+ element_scene.vector.getZ() * element_scene.vector.getZ());
angle = Math.acos(angle);
dist =
Math.abs(element_scene.pointDistance(element_modeltr.origin))
+ Math.abs(element_modeltr.origin.getDistance(element_scene.origin));
total_dist = angle + alpha * Math.abs(dist);
if (total_dist < less_total) {
less_total = total_dist;
closests[count] = count2;
}
}
distances[count] = Math.exp(-(less_total * less_total) / (variance));
total += distances[count];
element_model = model[count];
element_scene = elements_scene[closests[count]];
projection = element_scene.pointProjection(element_model.origin);
traslations[0][count] = projection.getX() - element_model.origin.getX();
traslations[1][count] = projection.getZ() - element_model.origin.getZ();
}
ret[0] = 0;
ret[1] = 0;
if (total != 0)
for (count = 0; count < len2; count++) {
distances[count] /= total;
ret[0] += traslations[0][count] * distances[count];
ret[1] += traslations[1][count] * distances[count];
}
return ret;
}
private MyTransform2D findClosestRotation(
Plane3D[] elements_scene,
Plane3D[] model,
Plane3D[] modeltr,
int[] closests,
double variance) {
MyTransform2D ret = new MyTransform2D();
double angle_res = 0;
double total = 0;
double[] distances;
double[] angles;
Plane3D element_scene;
Plane3D element_model;
Plane3D element_modeltr;
double angle;
double dist;
double total_dist;
double less_total;
double less_angle = 100;
double less_dist = 100;
int len, len2;
int count, count2;
double dist_aux;
len = elements_scene.length;
len2 = model.length;
distances = new double[len2];
angles = new double[len2];
for (count = 0; count < len2; count++) {
element_modeltr = modeltr[count];
less_total = Double.MAX_VALUE;
for (count2 = 0; count2 < len; count2++) {
element_scene = elements_scene[count2];
angle =
element_modeltr.vector.getX() * element_scene.vector.getX()
+ element_modeltr.vector.getZ() * element_scene.vector.getZ();
angle /=
Math.sqrt(
element_modeltr.vector.getX() * element_modeltr.vector.getX()
+ element_modeltr.vector.getZ() * element_modeltr.vector.getZ())
* Math.sqrt(
element_scene.vector.getX() * element_scene.vector.getX()
+ element_scene.vector.getZ() * element_scene.vector.getZ());
angle = Math.acos(angle);
dist_aux = Math.abs(element_scene.pointDistance(element_modeltr.origin));
dist = dist_aux + element_scene.origin.getDistance(element_modeltr.origin);
total_dist = angle + beta * Math.abs(dist);
if (total_dist < less_total) {
less_total = total_dist;
less_angle = angle;
less_dist = dist;
closests[count] = count2;
}
}
distances[count] =
Math.exp(-(less_angle * less_angle) / (variance))
+ beta * Math.exp(-(less_dist * less_dist) / variance);
total += distances[count];
element_model = model[count];
element_scene = elements_scene[closests[count]];
angle =
element_model.vector.getX() * element_scene.vector.getX()
+ element_model.vector.getZ() * element_scene.vector.getZ();
angle /=
Math.sqrt(
element_model.vector.getX() * element_model.vector.getX()
+ element_model.vector.getZ() * element_model.vector.getZ())
* Math.sqrt(
element_scene.vector.getX() * element_scene.vector.getX()
+ element_scene.vector.getZ() * element_scene.vector.getZ());
// degenerate case: by precision errors, angle could be > 1
if (angle > 1) angle = 0;
else angle = Math.acos(angle);
if ((element_model.vector.getZ() * element_scene.vector.getX()
- element_model.vector.getX() * element_scene.vector.getZ())
< 0) angle = -angle;
angles[count] = angle;
}
if (total != 0)
for (count = 0; count < len2; count++) {
distances[count] /= total;
angle_res += angles[count] * distances[count];
}
ret.setRotation(angle_res);
return ret;
}
