Skip to content
Snippets Groups Projects
Commit 7708b067 authored by Radek Ošlejšek's avatar Radek Ošlejšek
Browse files

Further minor improvements

parent 9d39bf3a
No related branches found
No related tags found
No related merge requests found
...@@ -28,7 +28,7 @@ public class Icp { ...@@ -28,7 +28,7 @@ public class Icp {
private final List<IcpTransformation> transformations = new LinkedList<>(); private final List<IcpTransformation> transformations = new LinkedList<>();
private MeshFacet transformedFacet = null; private MeshFacet transformedFacet = null;
private int maxIteration = 10; private int maxIteration = 10;
private boolean scale; private boolean scale;
private double error = 0.05; private double error = 0.05;
...@@ -141,8 +141,10 @@ public class Icp { ...@@ -141,8 +141,10 @@ public class Icp {
meanD /= countOfNotNullPoints; meanD /= countOfNotNullPoints;
Vector3d translation = new Vector3d(mainCenter.x - comparedCenter.x, Vector3d translation = new Vector3d(
mainCenter.y - comparedCenter.y, mainCenter.z -comparedCenter.z); mainCenter.x - comparedCenter.x,
mainCenter.y - comparedCenter.y,
mainCenter.z - comparedCenter.z);
// END computing translation parameter // END computing translation parameter
...@@ -158,9 +160,11 @@ public class Icp { ...@@ -158,9 +160,11 @@ public class Icp {
max = i; max = i;
} }
} }
Quaternion q = new Quaternion(eigM.getElement(0, max), eigM.getElement(1, max), eigM.getElement(2, max), Quaternion q = new Quaternion(
eigM.getElement(0, max),
eigM.getElement(1, max),
eigM.getElement(2, max),
eigM.getElement(3, max)); eigM.getElement(3, max));
q = q.normalize(); q = q.normalize();
// END computing rotation parameter // END computing rotation parameter
...@@ -223,11 +227,13 @@ public class Icp { ...@@ -223,11 +227,13 @@ public class Icp {
} }
if(scale && !Double.isNaN(transformation.getScaleFactor())) { if(scale && !Double.isNaN(transformation.getScaleFactor())) {
meshPointPosition.set(rotation.getX() * transformation.getScaleFactor() + transformation.getTranslation().x, meshPointPosition.set(
rotation.getX() * transformation.getScaleFactor() + transformation.getTranslation().x,
rotation.getY() * transformation.getScaleFactor() + transformation.getTranslation().y, rotation.getY() * transformation.getScaleFactor() + transformation.getTranslation().y,
rotation.getZ() * transformation.getScaleFactor() + transformation.getTranslation().z); rotation.getZ() * transformation.getScaleFactor() + transformation.getTranslation().z);
} else { } else {
meshPointPosition.set(rotation.getX() + transformation.getTranslation().x, meshPointPosition.set(
rotation.getX() + transformation.getTranslation().x,
rotation.getY() + transformation.getTranslation().y , rotation.getY() + transformation.getTranslation().y ,
rotation.getZ() + transformation.getTranslation().z); rotation.getZ() + transformation.getTranslation().z);
} }
...@@ -273,8 +279,8 @@ public class Icp { ...@@ -273,8 +279,8 @@ public class Icp {
countOfNotNullPoints ++; countOfNotNullPoints ++;
} }
result.add(new Point3d(xN/countOfNotNullPoints,yN/countOfNotNullPoints,zN/countOfNotNullPoints)); result.add(new Point3d(xN/countOfNotNullPoints, yN/countOfNotNullPoints, zN/countOfNotNullPoints));
result.add(new Point3d(xC/countOfNotNullPoints,yC/countOfNotNullPoints,zC/countOfNotNullPoints)); result.add(new Point3d(xC/countOfNotNullPoints, yC/countOfNotNullPoints, zC/countOfNotNullPoints));
return result; return result;
} }
...@@ -287,7 +293,7 @@ public class Icp { ...@@ -287,7 +293,7 @@ public class Icp {
* @return Vector3d which represents coordinates of new point according to center. * @return Vector3d which represents coordinates of new point according to center.
*/ */
private Point3d relativeCoordinate(Point3d p, Point3d center) { private Point3d relativeCoordinate(Point3d p, Point3d center) {
return new Point3d(p.x - center.x,p.y - center.y, p.z - center.z); return new Point3d(p.x - center.x, p.y - center.y, p.z - center.z);
} }
/** /**
...@@ -297,10 +303,11 @@ public class Icp { ...@@ -297,10 +303,11 @@ public class Icp {
* @return Sum matrix of given point * @return Sum matrix of given point
*/ */
private Matrix4d sumMatrixComp(Point3d p) { private Matrix4d sumMatrixComp(Point3d p) {
return new Matrix4d(0, -p.x, -p.y, -p.z, return new Matrix4d(
p.x, 0, p.z, -p.y, 0, -p.x, -p.y, -p.z,
p.y, -p.z, 0, p.x, p.x, 0, p.z, -p.y,
p.z, p.y, -p.x,0 ); p.y, -p.z, 0, p.x,
p.z, p.y, -p.x, 0);
} }
/** /**
* Compute sum matrix of given point. Given point is point from main facet. * Compute sum matrix of given point. Given point is point from main facet.
...@@ -309,10 +316,11 @@ public class Icp { ...@@ -309,10 +316,11 @@ public class Icp {
* @return Sum matrix of given point * @return Sum matrix of given point
*/ */
private Matrix4d sumMatrixMain(Point3d p) { private Matrix4d sumMatrixMain(Point3d p) {
return new Matrix4d(0, -p.x, -p.y, -p.z, return new Matrix4d(
p.x, 0, -p.z, p.y, 0, -p.x, -p.y, -p.z,
p.y, p.z, 0, -p.x, p.x, 0, -p.z, p.y,
p.z, -p.y, p.x, 0); p.y, p.z, 0, -p.x,
p.z, -p.y, p.x, 0);
} }
/** /**
...@@ -322,9 +330,11 @@ public class Icp { ...@@ -322,9 +330,11 @@ public class Icp {
* @return Matrix of point. * @return Matrix of point.
*/ */
private Matrix4d pointToMatrix(Point3d p){ private Matrix4d pointToMatrix(Point3d p){
return new Matrix4d(p.x, p.y, p.z, 1, return new Matrix4d(
0, 0, 0, 0, 0, 0, p.x, p.y, p.z, 1,
0,0, 0, 0, 0, 0); 0, 0, 0, 0,
0, 0, 0, 0,
0, 0, 0, 0);
} }
} }
\ No newline at end of file
...@@ -8,11 +8,12 @@ import javax.vecmath.Matrix4d; ...@@ -8,11 +8,12 @@ import javax.vecmath.Matrix4d;
* @author Maria Kocurekova * @author Maria Kocurekova
*/ */
public final class Quaternion { public final class Quaternion {
public final double w;
private final double w;
private final double x; private final double x;
private final double y; private final double y;
private final double z; private final double z;
/** /**
* Builds a quaternion from its components. * Builds a quaternion from its components.
* *
...@@ -21,10 +22,7 @@ public final class Quaternion { ...@@ -21,10 +22,7 @@ public final class Quaternion {
* @param y Second vector component. * @param y Second vector component.
* @param z Third vector component. * @param z Third vector component.
*/ */
public Quaternion( final double w, public Quaternion(double w, double x, double y, double z) {
final double x,
final double y,
final double z) {
this.w = w; this.w = w;
this.x = x; this.x = x;
this.y = y; this.y = y;
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment