Ask Your Question
0

In ROSJava,how can i transform the 'odom' frame to 'map' frame?

asked 2016-12-18 19:32:28 -0500

Tony10012 gravatar image
        I want to transform the 'odom' frame to the 'map' frame,but i do not know what i will do? In RosJava i saw a class "Transform".

package org.ros.rosjava_geometry;

import geometry_msgs.Pose;
import geometry_msgs.PoseStamped;
import org.ros.message.Time;
import org.ros.namespace.GraphName;
import org.ros.rosjava_geometry.Quaternion;
import org.ros.rosjava_geometry.Vector3;

public class Transform {
    private Vector3 translation;
    private Quaternion rotationAndScale;

    public static Transform fromTransformMessage(geometry_msgs.Transform message) {
        return new Transform(Vector3.fromVector3Message(message.getTranslation()), Quaternion.fromQuaternionMessage(message.getRotation()));
    }

    public static Transform fromPoseMessage(Pose message) {
        return new Transform(Vector3.fromPointMessage(message.getPosition()), Quaternion.fromQuaternionMessage(message.getOrientation()));
    }

    public static Transform identity() {
        return new Transform(Vector3.zero(), Quaternion.identity());
    }

    public static Transform xRotation(double angle) {
        return new Transform(Vector3.zero(), Quaternion.fromAxisAngle(Vector3.xAxis(), angle));
    }

    public static Transform yRotation(double angle) {
        return new Transform(Vector3.zero(), Quaternion.fromAxisAngle(Vector3.yAxis(), angle));
    }

    public static Transform zRotation(double angle) {
        return new Transform(Vector3.zero(), Quaternion.fromAxisAngle(Vector3.zAxis(), angle));
    }

    public static Transform translation(double x, double y, double z) {
        return new Transform(new Vector3(x, y, z), Quaternion.identity());
    }

    public static Transform translation(Vector3 vector) {
        return new Transform(vector, Quaternion.identity());
    }

    public Transform(Vector3 translation, Quaternion rotation) {
        this.translation = translation;
        this.rotationAndScale = rotation;
    }

    public Transform multiply(Transform other) {
        return new Transform(this.apply(other.translation), this.apply(other.rotationAndScale));
    }

    public Transform invert() {
        Quaternion inverseRotationAndScale = this.rotationAndScale.invert();
        return new Transform(inverseRotationAndScale.rotateAndScaleVector(this.translation.invert()), inverseRotationAndScale);
    }

    public Vector3 apply(Vector3 vector) {
        return this.rotationAndScale.rotateAndScaleVector(vector).add(this.translation);
    }

    public Quaternion apply(Quaternion quaternion) {
        return this.rotationAndScale.multiply(quaternion);
    }

    public Transform scale(double factor) {
        return new Transform(this.translation, this.rotationAndScale.scale(Math.sqrt(factor)));
    }

    public double getScale() {
        return this.rotationAndScale.getMagnitudeSquared();
    }

    public double[] toMatrix() {
        double x = this.rotationAndScale.getX();
        double y = this.rotationAndScale.getY();
        double z = this.rotationAndScale.getZ();
        double w = this.rotationAndScale.getW();
        double mm = this.rotationAndScale.getMagnitudeSquared();
        return new double[]{mm - 2.0D * y * y - 2.0D * z * z, 2.0D * x * y + 2.0D * z * w, 2.0D * x * z - 2.0D * y * w, 0.0D, 2.0D * x * y - 2.0D * z * w, mm - 2.0D * x * x - 2.0D * z * z, 2.0D * y * z + 2.0D * x * w, 0.0D, 2.0D * x * z + 2.0D * y * w, 2.0D * y * z - 2.0D * x * w, mm - 2.0D * x * x - 2.0D * y * y, 0.0D, this.translation.getX(), this.translation.getY(), this.translation.getZ(), 1.0D};
    }

    public geometry_msgs.Transform toTransformMessage(geometry_msgs.Transform result) {
        result.setTranslation(this.translation.toVector3Message(result.getTranslation()));
        result.setRotation(this.rotationAndScale.toQuaternionMessage(result.getRotation()));
        return result;
    }

    public Pose toPoseMessage(Pose result) {
        result.setPosition(this.translation.toPointMessage(result.getPosition()));
        result.setOrientation(this.rotationAndScale.toQuaternionMessage(result.getOrientation()));
        return result;
    }

    public PoseStamped toPoseStampedMessage(GraphName frame, Time stamp, PoseStamped result) {
        result.getHeader().setFrameId(frame.toString());
        result.getHeader().setStamp(stamp);
        result.setPose(this.toPoseMessage(result.getPose()));
        return result;
    }

    public boolean almostEquals(Transform other, double epsilon) {
        return this.translation.almostEquals(other.translation, epsilon) && this.rotationAndScale.almostEquals(other.rotationAndScale, epsilon);
    }

    public Vector3 getTranslation() {
        return ...
(more)
edit retag flag offensive close merge delete

1 Answer

Sort by » oldest newest most voted
0

answered 2016-12-20 03:29:03 -0500

Tony10012 gravatar image

updated 2016-12-20 03:29:58 -0500

I ues the method,maybe be not correct.

FrameTransform frameTransform = view.getFrameTransformTree().transform(GraphName.of("odom"), GraphName.of("map"));

See it link text

edit flag offensive delete link more

Your Answer

Please start posting anonymously - your entry will be published after you log in or create a new account.

Add Answer

Question Tools

2 followers

Stats

Asked: 2016-12-18 19:32:28 -0500

Seen: 311 times

Last updated: Dec 20 '16