bundles/common/org.eclipse.apogy.common.geometry.data3d/src-gen-custom/org/eclipse/apogy/common/geometry/data3d/impl/RigidBodyPoseTrackerCustomImpl.java - gerrit/apogy/apogy - Git at Google

 /*******************************************************************************
  * Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
  * All rights reserved. This program and the accompanying materials
  * are made available under the terms of the Eclipse Public License v1.0
  * which accompanies this distribution, and is available at
  * http://www.eclipse.org/legal/epl-v10.html
  *
  * Contributors:
  *     Pierre Allard,
  *     Regent L'Archeveque - initial API and implementation
  *
  * SPDX-License-Identifier: EPL-1.0
  *
  *******************************************************************************/
 package org.eclipse.apogy.common.geometry.data3d.impl;

 import java.lang.reflect.InvocationTargetException;
 import java.util.ArrayList;
 import java.util.List;

 import javax.vecmath.Matrix4d;

 import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFactory;
 import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DPackage;
 import org.eclipse.apogy.common.geometry.data3d.CartesianCoordinatesSet;
 import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
 import org.eclipse.apogy.common.geometry.data3d.PositionMarker;
 import org.eclipse.emf.common.util.EList;
 import org.eclipse.emf.ecore.util.EObjectResolvingEList;
 import org.slf4j.Logger;
 import org.slf4j.LoggerFactory;

 import Jama.Matrix;
 import Jama.SingularValueDecomposition;

 public class RigidBodyPoseTrackerCustomImpl extends RigidBodyPoseTrackerImpl {

 	private static final Logger Logger = LoggerFactory.getLogger(RigidBodyPoseTrackerImpl.class);

 	@Override
 	public EList<PositionMarker> getPositionMarkersAtOrigin() {
 		if (this.positionMarkersAtOrigin == null) {
 			this.positionMarkersAtOrigin = new EObjectResolvingEList<PositionMarker>(PositionMarker.class, this,
 					ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER__POSITION_MARKERS_AT_ORIGIN);
 		}

 		EList<PositionMarker> localCopy = this.positionMarkersAtOrigin;
 		return localCopy;

 	}

 	public Matrix4d computeTransformation(List<PositionMarker> listOfMarkerPositionsToTrack) throws Exception {
 		Matrix4d output = new Matrix4d();

 		if (listOfMarkerPositionsToTrack.size() < 3) {
 			throw new IllegalArgumentException("At least 3 position markers to track are needed");
 		}
 		if (this.positionMarkersAtOrigin.size() < 3) {
 			throw new IllegalArgumentException("At least 3 position markers at origin are needed");
 		}

 		CartesianCoordinatesSet markerPositionsAtOrigin = ApogyCommonGeometryData3DFactory.eINSTANCE
 				.createCartesianCoordinatesSet();
 		CartesianCoordinatesSet markerPositionsToTrack = ApogyCommonGeometryData3DFactory.eINSTANCE
 				.createCartesianCoordinatesSet();
 		CartesianPositionCoordinates tmpPos1 = ApogyCommonGeometryData3DFactory.eINSTANCE
 				.createCartesianPositionCoordinates();
 		CartesianPositionCoordinates tmpPos2 = ApogyCommonGeometryData3DFactory.eINSTANCE
 				.createCartesianPositionCoordinates();

 		for (int i = 0; i < this.positionMarkersAtOrigin.size(); i++) {
 			for (int j = 0; j < listOfMarkerPositionsToTrack.size(); j++) {
 				if (this.positionMarkersAtOrigin.get(i).getIdentifier() == listOfMarkerPositionsToTrack.get(j)
 						.getIdentifier()) {
 					tmpPos1 = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianPositionCoordinates();
 					tmpPos1.setX(this.positionMarkersAtOrigin.get(i).getX());
 					tmpPos1.setY(this.positionMarkersAtOrigin.get(i).getY());
 					tmpPos1.setZ(this.positionMarkersAtOrigin.get(i).getZ());
 					markerPositionsAtOrigin.getPoints().add(tmpPos1);

 					tmpPos2 = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianPositionCoordinates();
 					tmpPos2.setX(listOfMarkerPositionsToTrack.get(j).getX());
 					tmpPos2.setY(listOfMarkerPositionsToTrack.get(j).getY());
 					tmpPos2.setZ(listOfMarkerPositionsToTrack.get(j).getZ());
 					markerPositionsToTrack.getPoints().add(tmpPos2);
 				}
 			}
 		}

 		Matrix homogenousTransformationMatrix = compute3DRigidBodyTransformationBetweenTwo3DDatasets(
 				markerPositionsAtOrigin, markerPositionsToTrack);

 		// Convert Matrix to Matrix4d for output
 		for (int i = 0; i <= 3; i++) {
 			for (int j = 0; j <= 3; j++) {
 				output.setElement(i, j, homogenousTransformationMatrix.get(i, j));
 			}
 		}

 		return output;
 	}

 	public void addPositionMarkers(List<PositionMarker> marker) throws Exception {

 		if (this.positionMarkersAtOrigin == null) {
 			this.positionMarkersAtOrigin = new EObjectResolvingEList<PositionMarker>(PositionMarker.class, this,
 					ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER__POSITION_MARKERS_AT_ORIGIN);
 		}
 		for (int i = 0; i < marker.size(); i++) {
 			this.positionMarkersAtOrigin.add(marker.get(i));
 		}
 		// throw new UnsupportedOperationException();
 	}

 	public void removePositionMarkers(List<PositionMarker> markers) {
 		List<PositionMarker> localCopyMarker = markers;
 		for (int i = 0; i < localCopyMarker.size(); i++) {
 			for (int j = 0; j < this.positionMarkersAtOrigin.size(); j++) {
 				if (localCopyMarker.get(i).getIdentifier() == this.positionMarkersAtOrigin.get(j).getIdentifier()) {
 					this.positionMarkersAtOrigin.remove(j);
 				}
 			}
 		}
 		throw new UnsupportedOperationException();
 	}

 	private static Matrix compute3DRigidBodyTransformationBetweenTwo3DDatasets(CartesianCoordinatesSet p_0,
 			CartesianCoordinatesSet p) {
 		Matrix homogenousTransformationMatrix = new Matrix(4, 4);

 		// Compute the centroid of datasets
 		Matrix p_centroid = computeCentroidDataSet(p);
 		Matrix p_0_centroid = computeCentroidDataSet(p_0);

 		// Compute Rotation
 		Matrix rotationMatrix = computeRotationMatrixUsingSVD(p_0, p_0_centroid, p, p_centroid);

 		// Compute Translation
 		Matrix translationMatrix = computeTranslationMatrix(rotationMatrix, p_0_centroid, p_centroid);

 		// Pack rotation and translation components in an homogenous matrix
 		homogenousTransformationMatrix.setMatrix(0, 2, 0, 2, rotationMatrix);
 		homogenousTransformationMatrix.setMatrix(0, 2, 3, 3, translationMatrix);
 		homogenousTransformationMatrix.set(3, 3, 1.0);

 		return homogenousTransformationMatrix;
 	}

 	private static Matrix computeTranslationMatrix(Matrix rotationMatrix, Matrix p_0_centroid, Matrix p_centroid) {
 		Matrix translationMatrix = new Matrix(3, 1);

 		translationMatrix = p_centroid.minus(rotationMatrix.times(p_0_centroid));

 		return translationMatrix;
 	}

 	private static Matrix computeRotationMatrixUsingSVD(CartesianCoordinatesSet p_0, Matrix P_0_centroid,
 			CartesianCoordinatesSet p, Matrix P_centroid) {

 		// Convert input from CartesianCoordinatePosition to Matrix 3x1 and remove
 		// centroid
 		java.util.List<Matrix> mP_0 = new ArrayList<Matrix>();
 		java.util.List<Matrix> mP = new ArrayList<Matrix>();
 		Matrix tmpMat3x1 = new Matrix(3, 1);
 		for (int i = 0; i < p_0.getPoints().size(); i++) {
 			tmpMat3x1 = convertCartesianCoordinatePositionToMatrix3x1(p_0.getPoints().get(i)); // convert to Matrix
 			tmpMat3x1 = tmpMat3x1.minus(P_0_centroid); // offset the centroid
 			mP_0.add(tmpMat3x1);

 			tmpMat3x1 = convertCartesianCoordinatePositionToMatrix3x1(p.getPoints().get(i)); // convert to Matrix
 			tmpMat3x1 = tmpMat3x1.minus(P_centroid); // offset the centroid
 			mP.add(tmpMat3x1);
 		}

 		// Correlation Matrix H
 		Matrix H = new Matrix(3, 3);
 		Matrix tmp3x3 = new Matrix(3, 3);
 		for (int i = 0; i < mP.size(); i++) {
 			tmp3x3 = mP_0.get(i).times(mP.get(i).transpose());
 			H = H.plus(tmp3x3);
 		}

 		// SVD computation
 		SingularValueDecomposition svg = H.svd();

 		Matrix rotationMatrix = new Matrix(3, 3);
 		Matrix U = new Matrix(3, 3);
 		Matrix V = new Matrix(3, 3);

 		U = svg.getU();
 		V = svg.getV();
 		rotationMatrix = V.times(U.transpose());

 		double tol = 1e-4;
 		// if det(rotationMatrix)== -1, it means the dataset is planar or the amount of
 		// noise is large
 		if (Math.abs(rotationMatrix.det() + 1) < tol) {
 			Matrix V_prime = new Matrix(3, 3);
 			V_prime = V;
 			V_prime.set(0, 2, -1 * V.get(0, 2));
 			V_prime.set(1, 2, -1 * V.get(1, 2));
 			V_prime.set(2, 2, -1 * V.get(2, 2));
 			rotationMatrix = V_prime.times(U.transpose());
 		}

 		return rotationMatrix;
 	}

 	private static Matrix computeCentroidDataSet(CartesianCoordinatesSet p) {

 		Matrix centroid = new Matrix(3, 1);
 		double sumX = 0;
 		double sumY = 0;
 		double sumZ = 0;

 		for (int i = 0; i < p.getPoints().size(); i++) {
 			sumX += p.getPoints().get(i).getX();
 			sumY += p.getPoints().get(i).getY();
 			sumZ += p.getPoints().get(i).getZ();
 		}

 		centroid.set(0, 0, sumX / p.getPoints().size());
 		centroid.set(1, 0, sumY / p.getPoints().size());
 		centroid.set(2, 0, sumZ / p.getPoints().size());
 		return centroid;
 	}

 	private static Matrix convertCartesianCoordinatePositionToMatrix3x1(CartesianPositionCoordinates P) {
 		Matrix mP = new Matrix(3, 1);
 		mP.set(0, 0, P.getX());
 		mP.set(1, 0, P.getY());
 		mP.set(2, 0, P.getZ());

 		return mP;
 	}

 	@Override
 	@SuppressWarnings("unchecked")
 	public Object eInvoke(int operationID, EList<?> arguments) throws InvocationTargetException {
 		switch (operationID) {
 		case ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER___COMPUTE_TRANSFORMATION__ELIST:
 			try {
 				return computeTransformation((EList<PositionMarker>) arguments.get(0));
 			} catch (Exception e) {
 				Logger.error(e.getMessage(), e);
 			}
 		case ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER___ADD_POSITION_MARKERS__LIST:
 			try {
 				addPositionMarkers((List<PositionMarker>) arguments.get(0));
 			} catch (Exception e) {
 				Logger.error(e.getMessage(), e);
 			}
 			return null;
 		case ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER___REMOVE_POSITION_MARKERS__LIST:
 			removePositionMarkers((List<PositionMarker>) arguments.get(0));
 			return null;
 		}
 		return super.eInvoke(operationID, arguments);
 	}

 } // RigidBodyPoseTrackerImpl
	/*******************************************************************************
	* Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency
	* All rights reserved. This program and the accompanying materials
	* are made available under the terms of the Eclipse Public License v1.0
	* which accompanies this distribution, and is available at
	* http://www.eclipse.org/legal/epl-v10.html
	*
	* Contributors:
	* Pierre Allard,
	* Regent L'Archeveque - initial API and implementation
	*
	* SPDX-License-Identifier: EPL-1.0
	*
	*******************************************************************************/
	package org.eclipse.apogy.common.geometry.data3d.impl;

	import java.lang.reflect.InvocationTargetException;
	import java.util.ArrayList;
	import java.util.List;

	import javax.vecmath.Matrix4d;

	import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DFactory;
	import org.eclipse.apogy.common.geometry.data3d.ApogyCommonGeometryData3DPackage;
	import org.eclipse.apogy.common.geometry.data3d.CartesianCoordinatesSet;
	import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates;
	import org.eclipse.apogy.common.geometry.data3d.PositionMarker;
	import org.eclipse.emf.common.util.EList;
	import org.eclipse.emf.ecore.util.EObjectResolvingEList;
	import org.slf4j.Logger;
	import org.slf4j.LoggerFactory;

	import Jama.Matrix;
	import Jama.SingularValueDecomposition;

	public class RigidBodyPoseTrackerCustomImpl extends RigidBodyPoseTrackerImpl {

	private static final Logger Logger = LoggerFactory.getLogger(RigidBodyPoseTrackerImpl.class);

	@Override
	public EList<PositionMarker> getPositionMarkersAtOrigin() {
	if (this.positionMarkersAtOrigin == null) {
	this.positionMarkersAtOrigin = new EObjectResolvingEList<PositionMarker>(PositionMarker.class, this,
	ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER__POSITION_MARKERS_AT_ORIGIN);
	}

	EList<PositionMarker> localCopy = this.positionMarkersAtOrigin;
	return localCopy;

	}

	public Matrix4d computeTransformation(List<PositionMarker> listOfMarkerPositionsToTrack) throws Exception {
	Matrix4d output = new Matrix4d();

	if (listOfMarkerPositionsToTrack.size() < 3) {
	throw new IllegalArgumentException("At least 3 position markers to track are needed");
	}
	if (this.positionMarkersAtOrigin.size() < 3) {
	throw new IllegalArgumentException("At least 3 position markers at origin are needed");
	}

	CartesianCoordinatesSet markerPositionsAtOrigin = ApogyCommonGeometryData3DFactory.eINSTANCE
	.createCartesianCoordinatesSet();
	CartesianCoordinatesSet markerPositionsToTrack = ApogyCommonGeometryData3DFactory.eINSTANCE
	.createCartesianCoordinatesSet();
	CartesianPositionCoordinates tmpPos1 = ApogyCommonGeometryData3DFactory.eINSTANCE
	.createCartesianPositionCoordinates();
	CartesianPositionCoordinates tmpPos2 = ApogyCommonGeometryData3DFactory.eINSTANCE
	.createCartesianPositionCoordinates();

	for (int i = 0; i < this.positionMarkersAtOrigin.size(); i++) {
	for (int j = 0; j < listOfMarkerPositionsToTrack.size(); j++) {
	if (this.positionMarkersAtOrigin.get(i).getIdentifier() == listOfMarkerPositionsToTrack.get(j)
	.getIdentifier()) {
	tmpPos1 = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianPositionCoordinates();
	tmpPos1.setX(this.positionMarkersAtOrigin.get(i).getX());
	tmpPos1.setY(this.positionMarkersAtOrigin.get(i).getY());
	tmpPos1.setZ(this.positionMarkersAtOrigin.get(i).getZ());
	markerPositionsAtOrigin.getPoints().add(tmpPos1);

	tmpPos2 = ApogyCommonGeometryData3DFactory.eINSTANCE.createCartesianPositionCoordinates();
	tmpPos2.setX(listOfMarkerPositionsToTrack.get(j).getX());
	tmpPos2.setY(listOfMarkerPositionsToTrack.get(j).getY());
	tmpPos2.setZ(listOfMarkerPositionsToTrack.get(j).getZ());
	markerPositionsToTrack.getPoints().add(tmpPos2);
	}
	}
	}

	Matrix homogenousTransformationMatrix = compute3DRigidBodyTransformationBetweenTwo3DDatasets(
	markerPositionsAtOrigin, markerPositionsToTrack);

	// Convert Matrix to Matrix4d for output
	for (int i = 0; i <= 3; i++) {
	for (int j = 0; j <= 3; j++) {
	output.setElement(i, j, homogenousTransformationMatrix.get(i, j));
	}
	}

	return output;
	}

	public void addPositionMarkers(List<PositionMarker> marker) throws Exception {

	if (this.positionMarkersAtOrigin == null) {
	this.positionMarkersAtOrigin = new EObjectResolvingEList<PositionMarker>(PositionMarker.class, this,
	ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER__POSITION_MARKERS_AT_ORIGIN);
	}
	for (int i = 0; i < marker.size(); i++) {
	this.positionMarkersAtOrigin.add(marker.get(i));
	}
	// throw new UnsupportedOperationException();
	}

	public void removePositionMarkers(List<PositionMarker> markers) {
	List<PositionMarker> localCopyMarker = markers;
	for (int i = 0; i < localCopyMarker.size(); i++) {
	for (int j = 0; j < this.positionMarkersAtOrigin.size(); j++) {
	if (localCopyMarker.get(i).getIdentifier() == this.positionMarkersAtOrigin.get(j).getIdentifier()) {
	this.positionMarkersAtOrigin.remove(j);
	}
	}
	}
	throw new UnsupportedOperationException();
	}

	private static Matrix compute3DRigidBodyTransformationBetweenTwo3DDatasets(CartesianCoordinatesSet p_0,
	CartesianCoordinatesSet p) {
	Matrix homogenousTransformationMatrix = new Matrix(4, 4);

	// Compute the centroid of datasets
	Matrix p_centroid = computeCentroidDataSet(p);
	Matrix p_0_centroid = computeCentroidDataSet(p_0);

	// Compute Rotation
	Matrix rotationMatrix = computeRotationMatrixUsingSVD(p_0, p_0_centroid, p, p_centroid);

	// Compute Translation
	Matrix translationMatrix = computeTranslationMatrix(rotationMatrix, p_0_centroid, p_centroid);

	// Pack rotation and translation components in an homogenous matrix
	homogenousTransformationMatrix.setMatrix(0, 2, 0, 2, rotationMatrix);
	homogenousTransformationMatrix.setMatrix(0, 2, 3, 3, translationMatrix);
	homogenousTransformationMatrix.set(3, 3, 1.0);

	return homogenousTransformationMatrix;
	}

	private static Matrix computeTranslationMatrix(Matrix rotationMatrix, Matrix p_0_centroid, Matrix p_centroid) {
	Matrix translationMatrix = new Matrix(3, 1);

	translationMatrix = p_centroid.minus(rotationMatrix.times(p_0_centroid));

	return translationMatrix;
	}

	private static Matrix computeRotationMatrixUsingSVD(CartesianCoordinatesSet p_0, Matrix P_0_centroid,
	CartesianCoordinatesSet p, Matrix P_centroid) {

	// Convert input from CartesianCoordinatePosition to Matrix 3x1 and remove
	// centroid
	java.util.List<Matrix> mP_0 = new ArrayList<Matrix>();
	java.util.List<Matrix> mP = new ArrayList<Matrix>();
	Matrix tmpMat3x1 = new Matrix(3, 1);
	for (int i = 0; i < p_0.getPoints().size(); i++) {
	tmpMat3x1 = convertCartesianCoordinatePositionToMatrix3x1(p_0.getPoints().get(i)); // convert to Matrix
	tmpMat3x1 = tmpMat3x1.minus(P_0_centroid); // offset the centroid
	mP_0.add(tmpMat3x1);

	tmpMat3x1 = convertCartesianCoordinatePositionToMatrix3x1(p.getPoints().get(i)); // convert to Matrix
	tmpMat3x1 = tmpMat3x1.minus(P_centroid); // offset the centroid
	mP.add(tmpMat3x1);
	}

	// Correlation Matrix H
	Matrix H = new Matrix(3, 3);
	Matrix tmp3x3 = new Matrix(3, 3);
	for (int i = 0; i < mP.size(); i++) {
	tmp3x3 = mP_0.get(i).times(mP.get(i).transpose());
	H = H.plus(tmp3x3);
	}

	// SVD computation
	SingularValueDecomposition svg = H.svd();

	Matrix rotationMatrix = new Matrix(3, 3);
	Matrix U = new Matrix(3, 3);
	Matrix V = new Matrix(3, 3);

	U = svg.getU();
	V = svg.getV();
	rotationMatrix = V.times(U.transpose());

	double tol = 1e-4;
	// if det(rotationMatrix)== -1, it means the dataset is planar or the amount of
	// noise is large
	if (Math.abs(rotationMatrix.det() + 1) < tol) {
	Matrix V_prime = new Matrix(3, 3);
	V_prime = V;
	V_prime.set(0, 2, -1 * V.get(0, 2));
	V_prime.set(1, 2, -1 * V.get(1, 2));
	V_prime.set(2, 2, -1 * V.get(2, 2));
	rotationMatrix = V_prime.times(U.transpose());
	}

	return rotationMatrix;
	}

	private static Matrix computeCentroidDataSet(CartesianCoordinatesSet p) {

	Matrix centroid = new Matrix(3, 1);
	double sumX = 0;
	double sumY = 0;
	double sumZ = 0;

	for (int i = 0; i < p.getPoints().size(); i++) {
	sumX += p.getPoints().get(i).getX();
	sumY += p.getPoints().get(i).getY();
	sumZ += p.getPoints().get(i).getZ();
	}

	centroid.set(0, 0, sumX / p.getPoints().size());
	centroid.set(1, 0, sumY / p.getPoints().size());
	centroid.set(2, 0, sumZ / p.getPoints().size());
	return centroid;
	}

	private static Matrix convertCartesianCoordinatePositionToMatrix3x1(CartesianPositionCoordinates P) {
	Matrix mP = new Matrix(3, 1);
	mP.set(0, 0, P.getX());
	mP.set(1, 0, P.getY());
	mP.set(2, 0, P.getZ());

	return mP;
	}

	@Override
	@SuppressWarnings("unchecked")
	public Object eInvoke(int operationID, EList<?> arguments) throws InvocationTargetException {
	switch (operationID) {
	case ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER___COMPUTE_TRANSFORMATION__ELIST:
	try {
	return computeTransformation((EList<PositionMarker>) arguments.get(0));
	} catch (Exception e) {
	Logger.error(e.getMessage(), e);
	}
	case ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER___ADD_POSITION_MARKERS__LIST:
	try {
	addPositionMarkers((List<PositionMarker>) arguments.get(0));
	} catch (Exception e) {
	Logger.error(e.getMessage(), e);
	}
	return null;
	case ApogyCommonGeometryData3DPackage.RIGID_BODY_POSE_TRACKER___REMOVE_POSITION_MARKERS__LIST:
	removePositionMarkers((List<PositionMarker>) arguments.get(0));
	return null;
	}
	return super.eInvoke(operationID, arguments);
	}

	} // RigidBodyPoseTrackerImpl