StampedTransform.java
Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2011, Sjoerd van den Dries
00003  * All rights reserved.
00004  * 
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  * 
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Technische Universiteit Eindhoven nor the
00014  *       names of its contributors may be used to endorse or promote products
00015  *       derived from this software without specific prior written permission.
00016  * 
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 
00031 package tfjava;
00032 
00033 import ros.communication.Time;
00034 
00035 import javax.vecmath.Quat4d;
00036 import javax.vecmath.Vector3d;
00037 import javax.vecmath.Point3d;
00038 import javax.vecmath.Matrix4d;
00039 
00048 public class StampedTransform {       
00049         
00051     public String frameID;
00053     public String childFrameID;
00055     public Time timeStamp;
00057     protected Matrix4d transform;
00058         
00062     public StampedTransform(Vector3d translation, Quat4d rotation, Time timeStamp, String frameID, String childFrameID) {
00063         this.childFrameID = childFrameID; 
00064         this.frameID = frameID;
00065         this.timeStamp = timeStamp;        
00066         this.transform = new Matrix4d(rotation, translation, 1);
00067     }
00068     
00072     public StampedTransform(Matrix4d transform, Time timeStamp, String frameID, String childFrameID) {
00073         this.childFrameID = childFrameID; 
00074         this.frameID = frameID;
00075         this.timeStamp = timeStamp;
00076         this.transform = transform;
00077     }
00078     
00082     public static StampedTransform getIdentity() {
00083         Matrix4d identity = new Matrix4d();
00084         identity.setIdentity();        
00085         return new StampedTransform(identity, null, null, null);
00086     }
00087     
00091     public StampedTransform invert() {
00092         transform.invert();
00093         String mem = this.childFrameID;
00094         this.childFrameID = frameID;
00095         this.frameID = mem;
00096         return this;
00097     }
00098     
00102     public void mul(StampedTransform t1) {        
00103         this.transform.mul(t1.transform);
00104         this.childFrameID = t1.childFrameID;
00105     }
00106 
00110     public void mul(StampedTransform t1, StampedTransform t2) {
00111         this.transform.mul(t1.transform, t2.transform);
00112         this.frameID = t1.frameID;
00113         this.childFrameID = t2.childFrameID;
00114     }
00115     
00119     public void transformPose(Matrix4d pose, Matrix4d poseOut) {
00120         poseOut.mul(getMatrix4(), pose);
00121     }
00122     
00126     public void transformPoint(Point3d point, Point3d pointOut) {
00127         getMatrix4().transform(point, pointOut);
00128     }
00129     
00133     public void transformVector(Vector3d vector, Vector3d vectorOut) {
00134         getMatrix4().transform(vector, vectorOut);
00135     }
00136     
00140     public void transformPose(Stamped<Matrix4d> stampedIn, Stamped<Matrix4d> stampedOut) {
00141         if (stampedIn.frameID != frameID) {
00142             // TODO: throw error
00143             return;
00144         }
00145         transformPose(stampedIn.getData(), stampedOut.getData());
00146         stampedOut.frameID = childFrameID;
00147         stampedOut.timeStamp = new Time(timeStamp);
00148     }
00149     
00153     public void transformPoint(Stamped<Point3d> stampedIn, Stamped<Point3d> stampedOut) {
00154         if (stampedIn.frameID != frameID) {
00155             // TODO: throw error
00156             return;
00157         }
00158         transformPoint(stampedIn.getData(), stampedOut.getData());
00159         stampedOut.frameID = childFrameID;
00160         stampedOut.timeStamp = new Time(timeStamp);
00161     }  
00162     
00166     public void transformVector(Stamped<Vector3d> stampedIn, Stamped<Vector3d> stampedOut) {
00167         if (stampedIn.frameID != frameID) {
00168             // TODO: throw error
00169             return;
00170         }
00171         transformVector(stampedIn.getData(), stampedOut.getData());
00172         stampedOut.frameID = childFrameID;
00173         stampedOut.timeStamp = new Time(timeStamp);
00174     }          
00175  
00179     public Vector3d getTranslation() {
00180         Vector3d out = new Vector3d();
00181         transform.get(out);
00182         return out;
00183     }
00184     
00188     public void getTranslation(Vector3d out) {
00189         transform.get(out);
00190     }    
00194     public Quat4d getRotation() {
00195         Quat4d out = new Quat4d();
00196         transform.get(out);
00197         return out;
00198     }
00199     
00203     public void getRotation(Quat4d out) {
00204         transform.get(out);
00205     }
00206     
00210     public Matrix4d getMatrix4() {
00211         return transform;
00212     }      
00213     
00217     public String toString() {
00218         return "[" + frameID + " -> " + childFrameID + ", " + timeStamp + ", " + getMatrix4() + "]";
00219     } 
00220     
00221 }


tfjava
Author(s): Sjoerd van den Dries
autogenerated on Sun Oct 5 2014 22:54:19