Get a quaternion representing the orientation of the plane given by three points in space.

quaternion_from_points.py

#!/usr/bin/env python

import numpy as np
import math
from tf.transformations import euler_from_quaternion, quaternion_from_euler
from geometry_msgs.msg import Point

"""
Stuff to compute an orientation quaternion
from three points in the space (that represent a plane,
the quaternion represents the orientation of the plane).

Author: Sammy Pfeiffer <Sammy.Pfeiffer@student.uts.edu.au>
"""


def normalize(v):
    norm = np.linalg.norm(v)
    if norm == 0:
        return v
    return v / norm

# http://stackoverflow.com/questions/17044296/quaternion-rotation-without-euler-angles


def vectors_from_three_points(p1, p2, p3):
    """
    Given 3 points in the space, return the vectors
    p1->p2 and p1->p3.
    """
    if type(p1) == Point:
        x0, y0, z0 = p1.x, p1.y, p1.z
    elif type(p1) == list:
        x0, y0, z0 = p1[0], p1[1], p1[2]
    if type(p2) == Point:
        x1, y1, z1 = p2.x, p2.y, p2.z
    elif type(p2) == list:
        x1, y1, z1 = p2[0], p2[1], p2[2]
    if type(p3) == Point:
        x2, y2, z2 = p3.x, p3.y, p3.z
    elif type(p3) == list:
        x0, y0, z0 = p3[0], p3[1], p3[2]
    v0 = [x1 - x0, y1 - y0, z1 - z0]
    v1 = [x2 - x0, y2 - y0, z2 - z0]
    return v0, v1


def orthogonal_vectors_from_vectors(v0, v1):
    """
    Given two vectors that arent colinear return
    v0, the orthogonal vector to v0 and the crossproduct,
    and the crossproduct vector of v0 and v1.
    """
    # Two vectors v0 |/ v1
    v0 = normalize(v0)
    v1 = normalize(v1)
    # The cross product that points up |./
    vc = np.cross(v0, v1)
    # The orthogonal vector to the original v0 and to the crossproduct |._
    v0c = np.cross(v0, vc)
    return v0, v0c, vc


def quaternion_from_three_points(p1, p2, p3):
    """
    Given three points p1, p2, p3 that form a plane (cant be colinear)
    return the quaternion that represents the orientation
    of the normal of the plane.

    The orientation of the quaternion aligns:
        Z axis with the crossproduct (vc) of the vector p1->p2 (v0) and p1->p3 (v1) 'up'
        Y axis with the vector p1->p2 (v0)
        X axis with the orthogonal vector to the vector p1->p2 (v0) and the crossproduct (vc)

    :param p1 Point or list: Point or list(x, y, z)
    :param p2 Point or list: Point or list(x, y, z)
    :param p3 Point or list: Point or list(x, y, z)
    """
    v0, v1 = vectors_from_three_points(p1, p2, p3)
    # original is p1->p2, orthogonal is orthog to p1->p2 and to up,
    # up is cross product of p1->p2 and p1->p3
    original, orthogonal, up = orthogonal_vectors_from_vectors(v0, v1)
    # Align X to v0 and Z to vc
    q = quaternion_from_forward_and_up_vectors(up, original)
    return q


def quaternion_from_forward_and_up_vectors(forward, up):
    """
    Inspired in the Unity LookRotation Quaternion function,
    https://answers.unity.com/questions/467614/what-is-the-source-code-of-quaternionlookrotation.html
    this returns a quaternion from two orthogonal vectors (x, y, z)
    representing forward and up.
    """
    v0 = normalize(forward)
    v1 = normalize(np.cross(normalize(up), v0))
    v2 = np.cross(v0, v1)
    m00, m01, m02 = v1
    m10, m11, m12 = v2
    m20, m21, m22 = v0

    num8 = (m00 + m11) + m22

    if num8 > 0.0:
        num = math.sqrt(num8 + 1.0)
        w = num * 0.5
        num = 0.5 / num
        x = (m12 - m21) * num
        y = (m20 - m02) * num
        z = (m01 - m10) * num
        return x, y, z, w

    if (m00 >= m11) and (m00 >= m22):
        num7 = math.sqrt(((1.0 + m00) - m11) - m22)
        num4 = 0.5 / num7
        x = 0.5 * num7
        y = (m01 + m10) * num4
        z = (m02 + m20) * num4
        w = (m12 - m21) * num4
        return x, y, z, w

    if m11 > m22:
        num6 = math.sqrt(((1.0 + m11) - m00) - m22)
        num3 = 0.5 / num6
        x = (m10 + m01) * num3
        y = 0.5 * num6
        z = (m21 + m12) * num3
        w = (m20 - m02) * num3
        return x, y, z, w

    num5 = math.sqrt(((1.0 + m22) - m00) - m11)
    num2 = 0.5 / num5
    x = (m20 + m02) * num2
    y = (m21 + m12) * num2
    z = 0.5 * num5
    w = (m01 - m10) * num2
    return x, y, z, w


if __name__ == '__main__':
    # Tests with Rviz
    import rospy
    # Add to your workspace: https://github.com/DavidB-CMU/rviz_tools_py
    from rviz_tools_py import RvizMarkers
    from visualization_msgs.msg import Marker
    from geometry_msgs.msg import Polygon, Point32, Pose, Quaternion
    import random
    rospy.init_node('teststuff')
    rm = RvizMarkers('base_link', 'teststuff')
    rospy.sleep(1.0)

    def visualize(v1, v2, v3, rm):
        p1 = Point(*v1)
        p2 = Point(*v2)
        p3 = Point(*v3)

        rm.publishSphere(p1, 'red', 0.1, None)
        rm.publishText(p1, 'p1', 'red', 0.2, None)
        rm.publishSphere(p2, 'red', 0.1, None)
        rm.publishText(p2, 'p2', 'red', 0.2, None)
        rm.publishSphere(p3, 'red', 0.1, None)
        rm.publishText(p3, 'p3', 'red', 0.2, None)

        pol = Polygon()
        pol.points.append(Point32(*v1))
        pol.points.append(Point32(*v2))
        pol.points.append(Point32(*v3))

        rm.publishPolygon(pol, 'pink', 0.01, None)

        v0, v1 = vectors_from_three_points(p1, p2, p3)

        # Create an arrow from start and end points
        m = Marker()
        m.header.frame_id = 'base_link'
        m.type = m.ARROW
        m.action = m.ADD
        # v0
        m.points.append(p1)
        m.points.append(p2)
        m.color.a = 0.3
        m.color.g = 1.0
        m.scale.x = 0.05
        m.scale.y = 0.1
        m.scale.z = 0.0
        m.id = 9999 + random.randint(1, 100000)
        m.lifetime = rospy.Duration(100.0)
        rm.publishMarker(m)
        v0p = Point()
        v0p.x = p1.x + p2.x / 2.0
        v0p.y = p1.y + p2.y / 2.0
        v0p.z = p1.z + p2.z / 2.0
        rm.publishText(v0p, 'v0', 'green', 0.2, None)

        m2 = Marker()
        m2.header.frame_id = 'base_link'
        m2.type = m2.ARROW
        m2.action = m2.ADD
        # v1
        m2.points.append(p1)
        m2.points.append(p3)
        m2.color.a = 0.3
        m2.color.g = 1.0
        m2.scale.x = 0.05
        m2.scale.y = 0.1
        m2.scale.z = 0.0
        m2.id = 999 + random.randint(1, 100000)
        m2.lifetime = rospy.Duration(100.0)
        rm.publishMarker(m2)
        v1p = Point()
        v1p.x = p1.x + p3.x / 2.0
        v1p.y = p1.y + p3.y / 2.0
        v1p.z = p1.z + p3.z / 2.0
        rm.publishText(v1p, 'v1', 'green', 0.2, None)

        # Cross product vector
        c = np.cross(v0, v1)
        pc = Point(*c)
        m3 = Marker()
        m3.header.frame_id = 'base_link'
        m3.type = m3.ARROW
        m3.action = m3.ADD
        # v1
        m3.points.append(p1)
        m3.points.append(pc)
        m3.color.a = 0.3
        m3.color.g = 1.0
        m3.scale.x = 0.05
        m3.scale.y = 0.1
        m3.scale.z = 0.0
        m3.id = 9999
        m3.lifetime = rospy.Duration(100.0)
        rm.publishMarker(m3)
        vc = Point()
        vc.x = p1.x + pc.x / 2.0
        vc.y = p1.y + pc.y / 2.0
        vc.z = p1.z + pc.z / 2.0
        rm.publishText(vc, 'vc', 'green', 0.2, None)

        original, orthogonal, up = orthogonal_vectors_from_vectors(v0, v1)

        mforward = Marker()
        mforward.header.frame_id = 'base_link'
        mforward.type = mforward.ARROW
        mforward.action = mforward.ADD
        # forward vector
        mforward.points.append(p1)
        pforward = Point(*original)
        mforward.points.append(pforward)
        mforward.color.a = 0.4
        mforward.color.g = 0.5
        mforward.color.r = 0.5
        mforward.scale.x = 0.08
        mforward.scale.y = 0.1
        mforward.scale.z = 0.0
        mforward.id = 9994 + random.randint(1, 100000)
        mforward.lifetime = rospy.Duration(100.0)
        rm.publishMarker(mforward)
        vforp = Point()
        vforp.x = p1.x + pforward.x / 2.0
        vforp.y = p1.y + pforward.y / 2.0
        vforp.z = p1.z + pforward.z / 2.0
        rm.publishText(vforp, 'original', 'orange', 0.2, None)

        mup = Marker()
        mup.header.frame_id = 'base_link'
        mup.type = mup.ARROW
        mup.action = mup.ADD
        # forward vector
        mup.points.append(p1)
        pup = Point(*up)
        mup.points.append(pup)
        mup.color.a = 0.4
        mup.color.g = 0.5
        mup.color.r = 0.5
        mup.scale.x = 0.08
        mup.scale.y = 0.1
        mup.scale.z = 0.0
        mup.id = 9994 + random.randint(1, 100000)
        mup.lifetime = rospy.Duration(100.0)
        rm.publishMarker(mup)
        vupp = Point()
        vupp.x = p1.x + pup.x / 2.0
        vupp.y = p1.y + pup.y / 2.0
        vupp.z = p1.z + pup.z / 2.0
        rm.publishText(vupp, 'up', 'orange', 0.2, None)

        mortho = Marker()
        mortho.header.frame_id = 'base_link'
        mortho.type = mortho.ARROW
        mortho.action = mortho.ADD
        # forward vector
        mortho.points.append(p1)
        portho = Point(*orthogonal)
        mortho.points.append(portho)
        mortho.color.a = 0.4
        mortho.color.g = 0.5
        mortho.color.r = 0.5
        mortho.scale.x = 0.08
        mortho.scale.y = 0.1
        mortho.scale.z = 0.0
        mortho.id = 99494 + random.randint(1, 100000)
        mortho.lifetime = rospy.Duration(100.0)
        rm.publishMarker(mortho)
        vorthop = Point()
        vorthop.x = p1.x + portho.x / 2.0
        vorthop.y = p1.y + portho.y / 2.0
        vorthop.z = p1.z + portho.z / 2.0
        rm.publishText(vorthop, 'orthogonal', 'orange', 0.2, None)

        # Align X to vc and Y to v0
        # q = quaternion_from_forward_and_up_vectors(original, orthogonal)
        # roll, pitch, yaw = euler_from_quaternion(q)
        # q = quaternion_from_euler(roll + math.radians(90), pitch, yaw)

        # Align X to v0 and Y to __, Z to vc
        q = quaternion_from_forward_and_up_vectors(up, original)
        # roll, pitch, yaw = euler_from_quaternion(q)
        # q = quaternion_from_euler(roll, pitch, yaw + math.radians(90))

        final_pose = Pose(p1, Quaternion(*q))
        rm.publishAxis(final_pose, 0.3, 0.05, None)

    rm.deleteAllMarkers()
    rospy.sleep(0.4)

    # v1 = [0.0, 0.0, 0.0]
    # v2 = [1.0, 0.0, 0.0]
    # v3 = [0.0, 1.0, 0.0]
    # visualize(v1, v2, v3, rm)

    # v1 = [0.0, 0.0, 0.0]
    # v2 = [0.0, 1.0, 0.0]
    # v3 = [0.0, 0.0, 1.0]
    # visualize(v1, v2, v3, rm)

    # v1 = [0.0, 0.0, 0.0]
    # v2 = [1.0, 1.0, 0.0]
    # v3 = [1.0, 0.0, 0.0]
    # visualize(v1, v2, v3, rm)

    # v1 = [0.0, 0.0, 0.0]
    # v2 = [1.0, 1.0, 0.0]
    # v3 = [1.0, 0.0, 1.0]
    # visualize(v1, v2, v3, rm)

    # v1 = [0.0, 0.0, 0.0]
    # v2 = [1.0, 0.0, 0.0]
    # v3 = [0.0, 1.0, 1.0]
    # visualize(v1, v2, v3, rm)

    v1 = [0.0, 0.0, 0.0]
    v2 = [1.0, 0.5, 0.3]
    v3 = [0.3, 0.4, 1.0]
    visualize(v1, v2, v3, rm)

    rospy.spin()

Thank you, that is very useful!