I saw few similar post to this question, but none that provided a concrete final working solution.
I'm working with OptiTrack with python, Motive 2.2.0, NatNet SDK 4.0 using the NatNetClient from the examples provided with the SDK.
The coordinates system is such that Y is Up, X is backward and Z is left.
I want to translate the quaternion to a coordinates system in which X is forward, Y is right (or left, the more simple one) and Z is up.
I'm getting the quaternion qx, qy, qz, qw values, I think that in this order but I'm not sure (if you can find it in the documentation of Motive/OptiTrack it also could help).
Now I'm trying by a plenty of similar ways that I think should work to get the Euler angles: pitch, roll and yaw, and then check for the three rotations in which I should get 0 to -180 and then to 180 and back to 0 (or vice versa), but it is always results in roll direction which goes from 0 to 90 then back to 0 (positive grow and then positive decrease) and then to -90 and then again back to 0 (negative decrease and then negative grow).
Correct me on this too, but I that is the result that serves as the sanity check for assurance the transformation to Euler was done correctly, right ?
First I take the pose and the quat and create an SO(3) matrix (just for convenience) :
def pos_quat2SE(quat_data):
# Assumed quat_data order is (pos, quat)
SO = R.from_quat(quat_data[3:7]).as_matrix()
SE = np.matrix(np.eye(4))
SE[0:3,0:3] = np.matrix(SO)
SE[0:3,3] = np.matrix(quat_data[0:3]).T
return SE_motive
where quat_data is a simple concatenation of pos (3 values) and quat (4 values) as mentioned.
I tried to use scipy function:
from scipy.spatial.transform import Rotation as R
euler_transformed = R.from_matrix(SE_motive[0:3, 0:3]).as_euler('zyx', degrees=False)
but I'm not sure what should be the right argument for as_euler.
Also tried to use the following approach using this auxiliary function:
def SE_motive2transoform(SE_motive):
T_Yup2NED_inv = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
T_Yup2NED = invert_SE(T_Yup2NED_inv)
SE_transformed = SE_motive # T_Yup2NED
return SE_transformed
The next two tries gave the same result:
using this functions which should be equivalent:
def euler_from_quaternion(x, y, z, w):
"""
Convert a quaternion into euler angles (roll, pitch, yaw)
roll is rotation around x in radians (counterclockwise)
pitch is rotation around y in radians (counterclockwise)
yaw is rotation around z in radians (counterclockwise)
"""
t0 = +2.0 * (w * x + y * z)
t1 = +1.0 - 2.0 * (x * x + y * y)
roll_x = math.atan2(t0, t1)
t2 = +2.0 * (w * y - z * x)
t2 = +1.0 if t2 > +1.0 else t2
t2 = -1.0 if t2 < -1.0 else t2
pitch_y = math.asin(t2)
t3 = +2.0 * (w * z + x * y)
t4 = +1.0 - 2.0 * (y * y + z * z)
yaw_z = math.atan2(t3, t4)
return roll_x, pitch_y, yaw_z # in radians
def quaternion_to_rotation_matrix(q):
"""Return a 3x3 rotation matrix representing the orientation specified by a quaternion in x,y,z,w format.
The matrix is a Python list of lists.
"""
x = q[0]
y = q[1]
z = q[2]
w = q[3]
return [[w * w + x * x - y * y - z * z, 2 * (x * y - w * z), 2 * (x * z + w * y)],
[2 * (x * y + w * z), w * w - x * x + y * y - z * z, 2 * (y * z - w * x)],
[2 * (x * z - w * y), 2 * (y * z + w * x), w * w - x * x - y * y + z * z]]
on the input new_quat = np.vstack([quato[0], -quato[2], quato[1], quato[3]]) where qauto is the returned quaternion from the motive system in its mentioned above coordinate system. As much as I understand rearrangement of the quaternion values in that way should give me them in an xyz coordinate system and then I should been able to use the above function or even as_euler with xyz argument and etc. but it didn't work.
What is the shortest, working and elegant way to achieve the transform with the sanity check working of course ?
can it be done in that fashion:
def SE_motive2transoform(SE_motive):
T_Yup2NED_inv = np.array([[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]])
T_Yup2NED = invert_SE(T_Yup2NED_inv)
SE_transformed = SE_motive # T_Yup2NED
return SE_transformed
Thank you in advance.
I am working on a Spherical Map Script that generates a linear gradient along the y-axis and an angular gradient around the y-axis in the green and red channels, respectively. I have been unable to find any documentation suited to this online, but have experimented using examples in Javascript and C#. The linear gradient has worked out fine, but the angular gradient (one describing a 360 degree arc around the y-axis) continues to elude me.
My working script is as follows.
-- 3d sphere
-- produces rgb gradudient mapped to a 3d sphere, but not correctly.
-- this is basically missing an angle gradient around the y-axis...
function prepare()
-- tilt & rotation precalc
toRad = 180/math.pi
-- toCir = 360/math.p -- may or may not work for circumference...
radius = get_slider_input(RADIUS)
angle = get_angle_input(ROTATION)/toRad
cosa = math.cos(angle)
sina = math.sin(angle)
tilt = get_angle_input(TILT)/toRad
cosa2 = math.cos(tilt)
sina2 = math.sin(tilt)
end;
function get_sample(x, y)
local r, g, b, a = get_sample_map(x, y, SOURCE)
-- color gradient example
-- local r = x
-- local g = y
-- local b = (x + y) / 2
-- local a = 1
-- spherical mapping formulae (polar to cartesian, apparently)
-- local x = x * math.pi -- * aspect
-- local y = y * math.pi
-- local nx = math.cos(x) * math.sin(y)
-- local ny = math.sin(x) * math.sin(y)
-- local nz = math.cos(y)
-- cartesian to polar (reference)
-- example 1
-- r = math.sqrt(((x * x) + (y * y) + (z * z)))
-- long = math.acos(x / math.sqrt((x * x) + (y * y))) * (y < 0 ? -1 : 1)
-- lat = math.acos(z / radius) * (z < 0 ? -1 : 1)
-- example 2
-- r = math.sqrt((x * x) + (y * y) + (z * z))
-- long = math.acos(x / math.sqrt((x * x) + (y * y))) * (y < 0 ? -1 : 1)
-- lat = math.acos(z / r)
-- equations cannot accept boolean comparison
-- boolean syntax may not be valid in lua
-- image generation
-- shift origin to center and set radius limits
local px = (x*2.0) - 1.0
local py = (y*2.0) - 1.0
px = px/radius
py = py/radius
local len = math.sqrt((px*px)+(py*py))
if len > 1.0 then return 0,0,0,0 end
local z = -math.sqrt(1.0 - ((px*px)+(py*py)))
-- cartesian to polar
-- r = math.sqrt((x * x) + (y * y) + (z * z))
-- lat = math.acos(z / r)
-- long = math.acos(x / math.sqrt((x * x) + (y * y))) * (y < 0) ? -1 : 1)
-- apply rotaqtion and tilt (order is important)
local tz = (cosa2 * z) - (sina2 * py)
local ty = (sina2 * z) + (cosa2 * py) -- gradient along y-axis is correct
z = tz
py = ty
local tx = (cosa * px) - (sina * z) -- gradient needs to go around y-axis
local tz = (sina * px) + (cosa * z)
px = tx
z = tz
-- r = math.sqrt((x * x) + (y * y) + (z * z))
-- lat = math.acos(z / r) -- invalid z for this; what is correct source?
-- long = math.acos(x / math.sqrt((x * x) + (y * y))) -- map -1 : 1 needed
-- long = (sina * px) + (cosa * z) -- ok; 2 full rotations
-- return r, g, b, a
-- return px,py,z,a
return px/2+.5,py/2+.5,z/2+.5,a
-- return px/2+.5,px/2+.5,px/2+.5,a
-- return long,long,long,a
-- return px/2+.5,py/2+.5,long,a
end;
I found a way to construct a spherical angular gradient from the hemispherical axis gradients I started with, by using three as the RBG channels in an RBG to HSL conversion. I've used this to map textures to spheres in Filter Forge in map scripts like this.
I thought I'd share the solution for anyone who's followed my question here.
Given a rectangle (w, h) and a pie slice with a radius less or equal to the smaller of both sides (w, h), a start angle and an end angle, how can I place the slice optimally in the rectangle so that it fills the room best (from an optical point of view, not mathematically speaking)?
I'm currently placing the pie slice's center in the center of the rectangle and use the half of the smaller of both rectangle sides as the radius. This leaves plenty of room for certain configurations.
Examples to make clear what I'm after, based on the precondition that the slice is drawn like a unit circle (i.e. 0 degrees on positive X axis, then running clock-wise):
A start angle of 0 and an end angle of PI would lead to a filled lower half of the rectangle and an empty upper half. A good solution here would be to move the center up by 1/4*h.
A start angle of 0 and an end angle of PI/2 would lead to a filled bottom right quarter of the rectangle. A good solution here would be to move the center point to the top left of the rectangle and to set the radius to the smaller of both rectangle sides.
This is fairly easy for the cases I've sketched but it becomes complicated when the start and end angles are arbitrary. I am searching for an algorithm which determines center of the slice and radius in a way that fills the rectangle best. Pseudo code would be great since I'm not a big mathematician.
The extrema of the bounding box of your arc are in the following format:
x + x0 * r = 0
x + x1 * r = w
y + y0 * r = 0
y + y1 * r = h
The values x0, x1, y0 and y1 are found by taking the minimum and maximum values of up to 7 points: any tangential points that are spanned (i.e. 0, 90, 180 and 270 degrees) and the end points of the two line segments.
Given the extrema of the axis-aligned bounding box of the arc (x0, y0), (x1, y1) the radius and center point can be calculated as follows:
r = min(w/(x1-x0), h/(y1-y0)
x = -x0 * r
y = -y0 * r
Here is an implementation written in Lua:
-- ensures the angle is in the range [0, 360)
function wrap(angle)
local x = math.fmod(angle, 2 * math.pi)
if x < 0 then
x = x + 2 * math.pi
end
return x
end
function place_arc(t0, t1, w, h)
-- find the x-axis extrema
local x0 = 1
local x1 = -1
local xlist = {}
table.insert(xlist, 0)
table.insert(xlist, math.cos(t0))
table.insert(xlist, math.cos(t1))
if wrap(t0) > wrap(t1) then
table.insert(xlist, 1)
end
if wrap(t0-math.pi) > wrap(t1-math.pi) then
table.insert(xlist, -1)
end
for _, x in ipairs(xlist) do
if x < x0 then x0 = x end
if x > x1 then x1 = x end
end
-- find the y-axis extrema
local ylist = {}
local y0 = 1
local y1 = -1
table.insert(ylist, 0)
table.insert(ylist, math.sin(t0))
table.insert(ylist, math.sin(t1))
if wrap(t0-0.5*math.pi) > wrap(t1-0.5*math.pi) then
table.insert(ylist, 1)
end
if wrap(t0-1.5*math.pi) > wrap(t1-1.5*math.pi) then
table.insert(ylist, -1)
end
for _, y in ipairs(ylist) do
if y < y0 then y0 = y end
if y > y1 then y1 = y end
end
-- calculate the maximum radius the fits in the bounding box
local r = math.min(w / (x1 - x0), h / (y1 - y0))
-- find x & y from the radius and minimum extrema
local x = -x0 * r
local y = -y0 * r
-- calculate the final axis-aligned bounding-box (AABB)
local aabb = {
x0 = x + x0 * r,
y0 = y + y0 * r,
x1 = x + x1 * r,
y1 = y + y1 * r
}
return x, y, r, aabb
end
function center_arc(x, y, aabb, w, h)
dx = (w - aabb.x1) / 2
dy = (h - aabb.y1) / 2
return x + dx, y + dy
end
t0 = math.rad(60)
t1 = math.rad(300)
w = 320
h = 240
x, y, r, aabb = place_arc(t0, t1, w, h)
x, y = center_arc(x, y, aabb, w, h)
print(x, y, r)
Example output:
Instead of pseudo code, I used python, but it should be usable. For this algorithm, I assume that startAngle < endAngle and that both are within [-2 * PI, 2 * PI]. If you want to use both within [0, 2 * PI] and let startAngle > endAngle, I would do:
if (startAngle > endAngle):
startAngle = startAngle - 2 * PI
So, the algorithm that comes to mind is to calculate the bounds of the unit arc and then scale to fit your rectangle.
The first is the harder part. You need to calculate 4 numbers:
Left: MIN(cos(angle), 0)
Right: MAX(cos(angle), 0)
Top: MIN(sin(angle),0)
Bottom: MAX(sin(angle),0)
Of course, angle is a range, so it's not as simple as this. However, you really only have to include up to 11 points in this calculation. The start angle, the end angle, and potentially, the cardinal directions (there are 9 of these going from -2 * PI to 2 * PI.) I'm going to define boundingBoxes as lists of 4 elements, ordered [left, right, top, bottom]
def IncludeAngle(boundingBox, angle)
x = cos(angle)
y = sin(angle)
if (x < boundingBox[0]):
boundingBox[0] = x
if (x > boundingBox[1]):
boundingBox[1] = x
if (y < boundingBox[2]):
boundingBox[2] = y
if (y > boundingBox[3]):
boundingBox[3] = y
def CheckAngle(boundingBox, startAngle, endAngle, angle):
if (startAngle <= angle and endAngle >= angle):
IncludeAngle(boundingBox, angle)
boundingBox = [0, 0, 0, 0]
IncludeAngle(boundingBox, startAngle)
IncludeAngle(boundingBox, endAngle)
CheckAngle(boundingBox, startAngle, endAngle, -2 * PI)
CheckAngle(boundingBox, startAngle, endAngle, -3 * PI / 2)
CheckAngle(boundingBox, startAngle, endAngle, -PI)
CheckAngle(boundingBox, startAngle, endAngle, -PI / 2)
CheckAngle(boundingBox, startAngle, endAngle, 0)
CheckAngle(boundingBox, startAngle, endAngle, PI / 2)
CheckAngle(boundingBox, startAngle, endAngle, PI)
CheckAngle(boundingBox, startAngle, endAngle, 3 * PI / 2)
CheckAngle(boundingBox, startAngle, endAngle, 2 * PI)
Now you've computed the bounding box of an arc with center of 0,0 and radius of 1. To fill the box, we're going to have to solve a linear equation:
boundingBox[0] * xRadius + xOffset = 0
boundingBox[1] * xRadius + xOffset = w
boundingBox[2] * yRadius + yOffset = 0
boundingBox[3] * yRadius + yOffset = h
And we have to solve for xRadius and yRadius. You'll note there are two radiuses here. The reason for that is that in order to fill the rectangle, we have to multiple by different amounts in the two directions. Since your algorithm asks for only one radius, we will just pick the lower of the two values.
Solving the equation gives:
xRadius = w / (boundingBox[1] - boundingBox[0])
yRadius = h / (boundingBox[2] - boundingBox[3])
radius = MIN(xRadius, yRadius)
Here, you have to check for boundingBox[1] - boundingBox[0] being 0 and set xRadius to infinity in that case. This will give the correct result as yRadius will be smaller. If you don't have an infinity available, you can just set it to 0 and in the MIN function, check for 0 and use the other value in that case. xRadius and yRadius can't both be 0 because both sin and cos would have to be 0 for all angles included above for that to be the case.
Now we have to place the center of the arc. We want it centered in both directions. Now we'll create another linear equation:
(boundingBox[0] + boundingBox[1]) / 2 * radius + x = xCenter = w/2
(boundingBox[2] + boundingBox[3]) / 2 * radius + y = yCenter = h/2
Solving for x and y, the center of the arc, gives
x = w/2 - (boundingBox[0] + boundingBox[1]) * radius / 2
y = h/2 - (boundingBox[3] + boundingBox[2]) * radius / 2
This should give you the center of the arc and the radius needed to put the largest circle in the given rectangle.
I haven't tested any of this code, so this algorithm may have huge holes, or perhaps tiny ones caused by typos. I'd love to know if this algoritm works.
edit:
Putting all of the code together gives:
def IncludeAngle(boundingBox, angle)
x = cos(angle)
y = sin(angle)
if (x < boundingBox[0]):
boundingBox[0] = x
if (x > boundingBox[1]):
boundingBox[1] = x
if (y < boundingBox[2]):
boundingBox[2] = y
if (y > boundingBox[3]):
boundingBox[3] = y
def CheckAngle(boundingBox, startAngle, endAngle, angle):
if (startAngle <= angle and endAngle >= angle):
IncludeAngle(boundingBox, angle)
boundingBox = [0, 0, 0, 0]
IncludeAngle(boundingBox, startAngle)
IncludeAngle(boundingBox, endAngle)
CheckAngle(boundingBox, startAngle, endAngle, -2 * PI)
CheckAngle(boundingBox, startAngle, endAngle, -3 * PI / 2)
CheckAngle(boundingBox, startAngle, endAngle, -PI)
CheckAngle(boundingBox, startAngle, endAngle, -PI / 2)
CheckAngle(boundingBox, startAngle, endAngle, 0)
CheckAngle(boundingBox, startAngle, endAngle, PI / 2)
CheckAngle(boundingBox, startAngle, endAngle, PI)
CheckAngle(boundingBox, startAngle, endAngle, 3 * PI / 2)
CheckAngle(boundingBox, startAngle, endAngle, 2 * PI)
if (boundingBox[1] == boundingBox[0]):
xRadius = 0
else:
xRadius = w / (boundingBox[1] - boundingBox[0])
if (boundingBox[3] == boundingBox[2]):
yRadius = 0
else:
yRadius = h / (boundingBox[3] - boundingBox[2])
if xRadius == 0:
radius = yRadius
elif yRadius == 0:
radius = xRadius
else:
radius = MIN(xRadius, yRadius)
x = w/2 - (boundingBox[0] + boundingBox[1]) * radius / 2
y = h/2 - (boundingBox[3] + boundingBox[2]) * radius / 2
edit:
One issue here is that sin[2 * PI] is not going to be exactly 0 because of rounding errors. I think the solution is to get rid of the CheckAngle calls and replace them with something like:
def CheckCardinal(boundingBox, startAngle, endAngle, cardinal):
if startAngle < cardinal * PI / 2 and endAngle > cardinal * PI / 2:
cardinal = cardinal % 4
if cardinal == 0:
boundingBox[1] = 1
if cardinal == 1:
boundingBox[3] = 1
if cardinal == 2:
boundingBox[0] = -1
if cardinal == 3:
boundingBox[2] = -1
CheckCardinal(boundingBox, startAngle, endAngle, -4)
CheckCardinal(boundingBox, startAngle, endAngle, -3)
CheckCardinal(boundingBox, startAngle, endAngle, -2)
CheckCardinal(boundingBox, startAngle, endAngle, -1)
CheckCardinal(boundingBox, startAngle, endAngle, 0)
CheckCardinal(boundingBox, startAngle, endAngle, 1)
CheckCardinal(boundingBox, startAngle, endAngle, 2)
CheckCardinal(boundingBox, startAngle, endAngle, 3)
CheckCardinal(boundingBox, startAngle, endAngle, 4)
You still need IncludeAngle(startAngle) and IncludeAngle(endAngle)
Just consider a circle and forget the filling. The bounds will either be the center of the circle, the endpoints, or the points at 0, 90, 180, or 270 degrees (if they exist in this slice). The maxima and minima of these seven points will determine your bounding rectangle.
As far as placing it in the center, calculate the average of the max and min for both the rectangle and the pie slice, and add or subtract the difference of these to whichever one you want to move.
I would divide the problem into three steps:
Find the bounding box of a unit pie slice (or if a radius is given the actual pie slice centered at (0, 0)).
Fit the bounding box in your rectangle.
Use the information about fitting the bounding box to adjust the center and radius of the pie slice.
When I have time, I may flush this out with more details.