public void update(LongBuffer buffer) { rotation.setW(Double.longBitsToDouble(buffer.get())); rotation.setX(Double.longBitsToDouble(buffer.get())); rotation.setY(Double.longBitsToDouble(buffer.get())); rotation.setZ(Double.longBitsToDouble(buffer.get())); translation.setX(Double.longBitsToDouble(buffer.get())); translation.setY(Double.longBitsToDouble(buffer.get())); translation.setZ(Double.longBitsToDouble(buffer.get())); twist.setAngularPartX(Double.longBitsToDouble(buffer.get())); twist.setAngularPartY(Double.longBitsToDouble(buffer.get())); twist.setAngularPartZ(Double.longBitsToDouble(buffer.get())); twist.setLinearPartX(Double.longBitsToDouble(buffer.get())); twist.setLinearPartY(Double.longBitsToDouble(buffer.get())); twist.setLinearPartZ(Double.longBitsToDouble(buffer.get())); }
public void update(LongBuffer buffer) { rotation.setW(Double.longBitsToDouble(buffer.get())); rotation.setX(Double.longBitsToDouble(buffer.get())); rotation.setY(Double.longBitsToDouble(buffer.get())); rotation.setZ(Double.longBitsToDouble(buffer.get())); translation.setX(Double.longBitsToDouble(buffer.get())); translation.setY(Double.longBitsToDouble(buffer.get())); translation.setZ(Double.longBitsToDouble(buffer.get())); twist.setAngularPartX(Double.longBitsToDouble(buffer.get())); twist.setAngularPartY(Double.longBitsToDouble(buffer.get())); twist.setAngularPartZ(Double.longBitsToDouble(buffer.get())); twist.setLinearPartX(Double.longBitsToDouble(buffer.get())); twist.setLinearPartY(Double.longBitsToDouble(buffer.get())); twist.setLinearPartZ(Double.longBitsToDouble(buffer.get())); }
/** * Sets Euler parameters (unit quaternion) based on the given yaw, pitch and roll values. * @param yaw yaw rotation (about a fixed z-axis) * @param pitch pitch rotation (about a fixed y-axis) * @param roll roll rotation (about a fixed x-axis) * @param quaternionToPack Quat4d to set */ public static void convertYawPitchRollToQuaternion(double yaw, double pitch, double roll, Quat4d quaternionToPack) { double halfYaw = yaw / 2.0; double cYaw = Math.cos(halfYaw); double sYaw = Math.sin(halfYaw); double halfPitch = pitch / 2.0; double cPitch = Math.cos(halfPitch); double sPitch = Math.sin(halfPitch); double halfRoll = roll / 2.0; double cRoll = Math.cos(halfRoll); double sRoll = Math.sin(halfRoll); quaternionToPack.setW(cYaw * cPitch * cRoll + sYaw * sPitch * sRoll); quaternionToPack.setX(cYaw * cPitch * sRoll - sYaw * sPitch * cRoll); quaternionToPack.setY(sYaw * cPitch * sRoll + cYaw * sPitch * cRoll); quaternionToPack.setZ(sYaw * cPitch * cRoll - cYaw * sPitch * sRoll); }
quat.setX((rot21 - rot12) / val); quat.setY((rot02 - rot20) / val); quat.setZ((rot10 - rot01) / val); quat.setX((rot01 + rot10) / val); quat.setY(0.25 * val); quat.setZ((rot12 + rot21) / val); quat.setX(0.25 * val); quat.setY((rot01 + rot10) / val); quat.setZ((rot02 + rot20) / val); quat.setX((rot02 + rot20) / val); quat.setY((rot12 + rot21) / val); quat.setZ(0.25 * val);
orientation.setX(data.getDouble()); orientation.setY(data.getDouble()); orientation.setZ(data.getDouble());
quat.setX((mat21 - mat12) / val); quat.setY((mat02 - mat20) / val); quat.setZ((mat10 - mat01) / val); quat.setX((mat01 + mat10) / val); quat.setY(0.25 * val); quat.setZ((mat12 + mat21) / val); quat.setX(0.25 * val); quat.setY((mat01 + mat10) / val); quat.setZ((mat02 + mat20) / val); quat.setX((mat02 + mat20) / val); quat.setY((mat12 + mat21) / val); quat.setZ(0.25 * val);
public void exp(Vector3d rotationVector, Quat4d quaternionToPack) { double length = rotationVector.length(); if (length < 1.0e-7) { quaternionToPack.set(0.0, 0.0, 0.0, 1.0); return; } quaternionToPack.setW(Math.cos(0.5 * length)); double s = Math.sin(0.5 * length); vectorForExp.set(rotationVector); vectorForExp.scale(1.0 / length); quaternionToPack.setX(s * vectorForExp.getX()); quaternionToPack.setY(s * vectorForExp.getY()); quaternionToPack.setZ(s * vectorForExp.getZ()); }
public static void packRosQuaternionToQuat4d(Quaternion rosQuat, Quat4d quat) { quat.setX(rosQuat.getX()); quat.setY(rosQuat.getY()); quat.setZ(rosQuat.getZ()); quat.setW(rosQuat.getW()); }
rotationError.setX(rand.nextGaussian() * quaternionNoiseStd); rotationError.setY(rand.nextGaussian() * quaternionNoiseStd); rotationError.setZ(rand.nextGaussian() * quaternionNoiseStd);
public void compute() { double weight = 1.0 / quaternions.getNumRows(); outerProduct.reshape(4, 4); CommonOps.multAddTransA(weight, quaternions, quaternions, outerProduct); eigenDecomposition.decompose(outerProduct); double maxEigenValue = Double.NEGATIVE_INFINITY; DenseMatrix64F eigenVectorAssociatedWithMaxEigenValue = null; for (int i = 0; i < eigenDecomposition.getNumberOfEigenvalues(); i++) { double eigenValue = eigenDecomposition.getEigenValueAsDouble(i); if (eigenValue > maxEigenValue) { maxEigenValue = eigenValue; eigenVectorAssociatedWithMaxEigenValue = eigenDecomposition.getEigenVector(i); } } averageQuaternion.setX(eigenVectorAssociatedWithMaxEigenValue.get(0, 0)); averageQuaternion.setY(eigenVectorAssociatedWithMaxEigenValue.get(1, 0)); averageQuaternion.setZ(eigenVectorAssociatedWithMaxEigenValue.get(2, 0)); averageQuaternion.setW(eigenVectorAssociatedWithMaxEigenValue.get(3, 0)); }
quaternionToPack.setX(inv * (rotationMatrix.getElement(2, 1) - rotationMatrix.getElement(1, 2))); quaternionToPack.setY(inv * (rotationMatrix.getElement(0, 2) - rotationMatrix.getElement(2, 0))); quaternionToPack.setZ(inv * (rotationMatrix.getElement(1, 0) - rotationMatrix.getElement(0, 1))); quaternionToPack.setX(0.5 * FastMath.sqrt(s + 1.0)); double inv = 0.25 / quaternionToPack.getX(); quaternionToPack.setW(inv * (rotationMatrix.getElement(2, 1) - rotationMatrix.getElement(1, 2))); double inv = 0.25 / quaternionToPack.getY(); quaternionToPack.setW(inv * (rotationMatrix.getElement(0, 2) - rotationMatrix.getElement(2, 0))); quaternionToPack.setX(inv * (rotationMatrix.getElement(1, 0) + rotationMatrix.getElement(0, 1))); quaternionToPack.setZ(inv * (rotationMatrix.getElement(1, 2) + rotationMatrix.getElement(2, 1))); double inv = 0.25 / quaternionToPack.getZ(); quaternionToPack.setW(inv * (rotationMatrix.getElement(1, 0) - rotationMatrix.getElement(0, 1))); quaternionToPack.setX(inv * (rotationMatrix.getElement(2, 0) + rotationMatrix.getElement(0, 2))); quaternionToPack.setY(inv * (rotationMatrix.getElement(1, 2) + rotationMatrix.getElement(2, 1)));