public void interpolate(TimeStampedTransform3D timeStampedTransform1, TimeStampedTransform3D timeStampedTransform2, TimeStampedTransform3D resultToPack, long timeStamp) { long timeStamp1 = timeStampedTransform1.getTimeStamp(); long timeStamp2 = timeStampedTransform2.getTimeStamp(); MathTools.checkIntervalContains(timeStamp, timeStamp1, timeStamp2); RigidBodyTransform transform1 = timeStampedTransform1.getTransform3D(); RigidBodyTransform transform2 = timeStampedTransform2.getTransform3D(); resultToPack.setTimeStamp(timeStamp); double alpha = ((double) (timeStamp - timeStamp1)) / ((double) (timeStamp2 - timeStamp1)); RigidBodyTransform interpolatedTransform = resultToPack.getTransform3D(); computeInterpolation(transform1, transform2, interpolatedTransform, alpha); } }
public void interpolate(TimeStampedTransform3D timeStampedTransform1, TimeStampedTransform3D timeStampedTransform2, TimeStampedTransform3D resultToPack, long timeStamp) { long timeStamp1 = timeStampedTransform1.getTimeStamp(); long timeStamp2 = timeStampedTransform2.getTimeStamp(); MathTools.checkIfInRange(timeStamp, timeStamp1, timeStamp2); RigidBodyTransform transform1 = timeStampedTransform1.getTransform3D(); RigidBodyTransform transform2 = timeStampedTransform2.getTransform3D(); resultToPack.setTimeStamp(timeStamp); double alpha = ((double) (timeStamp - timeStamp1)) / ((double) (timeStamp2 - timeStamp1)); RigidBodyTransform interpolatedTransform = resultToPack.getTransform3D(); computeInterpolation(transform1, transform2, interpolatedTransform, alpha); } }
public void set(TimeStampedTransform3D other) { setTransform3D(other.getTransform3D()); setTimeStamp(other.getTimeStamp()); }
public void set(TimeStampedTransform3D other) { setTransform3D(other.getTransform3D()); setTimeStamp(other.getTimeStamp()); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testGetTransform() { TimeStampedTransform3D toBeTested = new TimeStampedTransform3D(); RigidBodyTransform expectedTransform = new RigidBodyTransform(); // Test that the getter returns the original transform and not a copy. toBeTested.getTransform3D().set(expectedTransform); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testConstructor() { Random random = new Random(3213620L); RigidBodyTransform expectedTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); long expectedTimestamp = RandomNumbers.nextInt(random, 132, 51568418); TimeStampedTransform3D toBeTested = new TimeStampedTransform3D(expectedTransform, expectedTimestamp); assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); assertTrue("TimestampedTransform should only copy the given transform into an internal variable", expectedTransform != toBeTested.getTransform3D()); }
public static StampedPosePacket createStampedPosePacket(String frameId, TimeStampedTransform3D transform, double confidenceFactor) { StampedPosePacket message = new StampedPosePacket(); message.getFrameId().append(frameId); message.getPose().set(transform.getTransform3D()); message.setTimestamp(transform.getTimeStamp()); message.setConfidenceFactor(confidenceFactor); return message; }
public synchronized void addTimeStampedTransform(TimeStampedTransform3D timeStampedTransform3D) { transforms[currentIndex].setTransform3D(timeStampedTransform3D.getTransform3D()); transforms[currentIndex].setTimeStamp(timeStampedTransform3D.getTimeStamp()); newestTimestamp = timeStampedTransform3D.getTimeStamp(); currentIndex++; if (currentIndex >= size) { currentIndex = 0; } if (transforms[currentIndex].getTimeStamp() == Long.MIN_VALUE) { oldestTimestamp = newestTimestamp; } else { oldestTimestamp = transforms[currentIndex].getTimeStamp(); } }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testSetters() { TimeStampedTransform3D toBeTested = new TimeStampedTransform3D(); RigidBodyTransform expectedTransform = new RigidBodyTransform(); long expectedTimestamp = 0; assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); Random random = new Random(3213620L); expectedTimestamp = RandomNumbers.nextInt(random, 132, 51568418); toBeTested.setTimeStamp(expectedTimestamp); assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); expectedTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); toBeTested.setTransform3D(expectedTransform); assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); expectedTimestamp = RandomNumbers.nextInt(random, 132, 51568418); expectedTransform = EuclidCoreRandomTools.nextRigidBodyTransform(random); TimeStampedTransform3D expectedTimeStampedTransform = new TimeStampedTransform3D(expectedTransform, expectedTimestamp); toBeTested.set(expectedTimeStampedTransform); assertEquals("Timestamp is different from what was expected", expectedTimeStampedTransform.getTimeStamp(), toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTimeStampedTransform.getTransform3D().epsilonEquals(toBeTested.getTransform3D(), EPSILON)); }
@ContinuousIntegrationTest(estimatedDuration = 0.0) @Test(timeout = 30000) public void testEmptyConstructor() { TimeStampedTransform3D toBeTested = new TimeStampedTransform3D(); RigidBodyTransform expectedTransform = new RigidBodyTransform(); long expectedTimestamp = 0; assertEquals("Timestamp is different from what was expected", expectedTimestamp, toBeTested.getTimeStamp()); assertTrue("Transform is different from what was expected", expectedTransform.epsilonEquals(toBeTested.getTransform3D(), EPSILON)); }
/** * Calculates the difference between the external at t with the state estimated pelvis pose at t and stores it * @param localizationPose - the corrected pelvis pose */ public void calculateAndStoreErrorInPast(TimeStampedTransform3D timestampedlocalizationPose) { long timeStamp = timestampedlocalizationPose.getTimeStamp(); RigidBodyTransform localizationPose = timestampedlocalizationPose.getTransform3D(); localizationPose.getTranslation(localizationTranslationInPast); newLocalizationTranslationFrame.setAndUpdate(localizationTranslationInPast); localizationPose.getRotation(localizationRotationInPast); newLocalizationRotationFrame.setAndUpdate(localizationRotationInPast); stateEstimatorPelvisPoseBuffer.findTransform(timeStamp, seTimeStampedPose); RigidBodyTransform sePose = seTimeStampedPose.getTransform3D(); sePose.getTranslation(seTranslationInPast); pelvisStateAtLocalizationTimeTranslationFrame.setAndUpdate(seTranslationInPast); sePose.getRotation(seRotationInPast); pelvisStateAtLocalizationTimeRotationFrame.setAndUpdate(seRotationInPast); newLocalizationTranslationFrame.getTransformToDesiredFrame(translationErrorInPastTransform, pelvisStateAtLocalizationTimeTranslationFrame); newLocalizationRotationFrame.getTransformToDesiredFrame(rotationErrorInPastTransform, pelvisStateAtLocalizationTimeRotationFrame); totalTranslationErrorFrame.setAndUpdate(translationErrorInPastTransform); totalRotationErrorFrame.setAndUpdate(rotationErrorInPastTransform); }
RigidBodyTransform stateEstimatorPose = timeStampedTransform3DToPack.getTransform3D(); RigidBodyTransform localizationPose = timeStampedExternalPose.getTransform3D();
/** * Compare the localizationTimeStampedTransform with the stateEstimatorTimeStampedTransform having the same timeStamp * @param localizationTimeStampedTransformInWorld */ public void updateLocalizationTransform(TimeStampedTransform3D localizationTimeStampedTransformInWorld) { //update the stateEstimator reference frame in the past stateEstimatorTimeStampedTransformBuffer.findTransform(localizationTimeStampedTransformInWorld.getTimeStamp(), stateEstimatorTimeStampedTransformInPast); stateEstimatorPoseInThePast.setPoseIncludingFrame(worldFrame, stateEstimatorTimeStampedTransformInPast.getTransform3D()); stateEstimatorReferenceFrameInThePast.setPoseAndUpdate(stateEstimatorPoseInThePast); //update the localization Pose localizationPoseInThePast.setPoseIncludingFrame(worldFrame, localizationTimeStampedTransformInWorld.getTransform3D()); localizationReferenceFrameInThePast.setPoseAndUpdate(localizationPoseInThePast); stateEstimatorPositionInThePastInWorldFrame.setToZero(stateEstimatorReferenceFrameInThePast); stateEstimatorPositionInThePastInWorldFrame.changeFrame(worldFrame); localizationPositionInThePastInWorldFrame.setToZero(localizationReferenceFrameInThePast); localizationPositionInThePastInWorldFrame.changeFrame(worldFrame); translationOffsetFrameVector.sub(localizationPositionInThePastInWorldFrame, stateEstimatorPositionInThePastInWorldFrame); translationOffsetFrameVector.get(translationOffsetVector); localizationPoseTransformInThePast_InStateEstimatorReferenceFrameInThePast_Translation.setTranslationAndIdentityRotation(translationOffsetVector); localizationPoseInThePast.changeFrame(stateEstimatorReferenceFrameInThePast); localizationPoseInThePast.getPose(localizationPoseTransformInThePast_InStateEstimatorReferenceFrameInThePast_Rotation); localizationPoseTransformInThePast_InStateEstimatorReferenceFrameInThePast_Rotation.zeroTranslation(); totalErrorTransform.set(localizationPoseTransformInThePast_InStateEstimatorReferenceFrameInThePast_Rotation); totalErrorTransform.setTranslation(translationOffsetVector); localizationReferenceFrameInPresent_Rotation.update(); }
transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp);
transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp);
transformInterpolationCalculator.computeInterpolation(firstTimeStampedTransform.getTransform3D(), secondTimeStampedTransform.getTransform3D(), expectedTransform, alpha); assertTrue(expectedTransform.epsilonEquals(toTestTimeStampedTransform.getTransform3D(), 1.0e-10));
public synchronized void onNewMessage(geometry_msgs.PoseStamped received) { // get timestamp timeStamp = received.getHeader().getStamp().totalNsecs(); // get Point Double posx = received.getPose().getPosition().getX(); Double posy = received.getPose().getPosition().getY(); Double posz = received.getPose().getPosition().getZ(); pos = new Vector3D(posx, posy, posz); // get Rotation Double rotx = received.getPose().getOrientation().getX(); Double roty = received.getPose().getOrientation().getY(); Double rotz = received.getPose().getOrientation().getZ(); Double rotw = received.getPose().getOrientation().getW(); rot = new Quaternion(rotx, roty, rotz, rotw); // get frameID frameID = received.getHeader().getFrameId(); TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp); newPose(frameID, transform); }
public synchronized void onNewMessage(geometry_msgs.PoseStamped received) { // get timestamp timeStamp = received.getHeader().getStamp().totalNsecs(); // get Point Double posx = received.getPose().getPosition().getX(); Double posy = received.getPose().getPosition().getY(); Double posz = received.getPose().getPosition().getZ(); pos = new Vector3d(posx, posy, posz); // get Rotation Double rotx = received.getPose().getOrientation().getX(); Double roty = received.getPose().getOrientation().getY(); Double rotz = received.getPose().getOrientation().getZ(); Double rotw = received.getPose().getOrientation().getW(); rot = new Quat4d(rotx, roty, rotz, rotw); // get frameID frameID = received.getHeader().getFrameId(); TimeStampedTransform3D transform = new TimeStampedTransform3D(); transform.getTransform3D().setTranslation(pos); transform.getTransform3D().setRotation(rot); transform.setTimeStamp(timeStamp); newPose(frameID, transform); }
stateEstimatorPelvisPoseBuffer.findTransform(timestamp, pelvisTimeStampedTransformInThePast); RigidBodyTransform pelvisTransformInPast_Translation = new RigidBodyTransform(pelvisTimeStampedTransformInThePast.getTransform3D()); RigidBodyTransform pelvisTransformInPast_Rotation = new RigidBodyTransform(pelvisTransformInPast_Translation); pelvisTransformInPast_Translation.setRotationToIdentity();
stateEstimatorPelvisPoseBuffer.findTransform(timestamp, pelvisTimeStampedTransformInThePast); RigidBodyTransform pelvisTransformInPast_Translation = new RigidBodyTransform(pelvisTimeStampedTransformInThePast.getTransform3D()); RigidBodyTransform pelvisTransformInPast_Rotation = new RigidBodyTransform(pelvisTransformInPast_Translation); pelvisTransformInPast_Translation.setRotationToZero();