We Keep Coding

Converting ArUco axis-angle to Unity3D Quaternion

I'm interested in comparing the quaternions of an object presented in the real-world (with ArUco marker on top of it) and its simulated version in Unity3D.
To do this, I generated different scenes in Unity with the object in different locations. I stored its position and orientation relative to the camera in a csv file. where quaternions is looking something like this (for one example):
[-0.492555320262909 -0.00628990028053522 0.00224017538130283 0.870255589485168]
In ArUco, after using estimatePoseSingleMarkers I got a compact version of Angle-Axis, and I converted it to Quaternion using the following function:
def find_quat(rvecs):
a = np.array(rvecs[0][0])
theta = math.sqrt(a[0]**2 + a[1]**2 + a[2]**2)
b = a/theta
qx = b[0] * math.sin(theta/2)
qy = -b[1] * math.sin(theta/2) # left-handed vs right handed
qz = b[2] * math.sin(theta/2)
qw = math.cos(theta/2)
print(qx, qy, qz, qw)
where rvecs is the return value of ArUco
However, after doing this I'm still getting way different results, example of the same scene:
[0.9464098048208864 -0.02661258975275046 -0.009733748408866453 0.321722715311581] << aruco result
[-0.492555320262909 -0.00628990028053522 0.00224017538130283 0.870255589485168] << Unity's result
Sample input to find_quat:
[[[ 2.4849011 0.04546755 -0.030406 ]]]
which is the output of estimatePoseSingleMarkers function
Unity's Quaternion is found as follows:
GameObject.Find("Cube").transform.localRotation;
Am I missing something?

For anyone coming here trying to find an answer.
My problem was that I was having the marker on top of the cube (so rotated by -90) which made converting the orientation impossible.
Change your pivot point in Unity and rotate it by -90. Then convert by
(x,y,z,w) = (-x,y,-z,w)

Solving natural convection equations (heat and flow) with the shooting method

TL;DR I've been implementing a python program to solve numerically equations for natural convection based on a particular similarity variable using runge-kutta 4 and the shooting method. However I don't get the right solutions when I plot it. Did I make a mistake somewhere ?
Hi !
Starting from a special case of natural convection, we get these similitude equations.
The first describe the fluid flow, the second describe the heat flow.
"Pr" is for Prandtl it's basically a dimensionless number used in fluid dynamics (Prandtl) :
These equations are subjects to the following boundary values such that the temperature near the plate is greater than the temperature outside the boundary layer and such that the fluid velocity is 0 far away from the boundary layer.
I've been trying to resolve these numerically with Runge-Kutta 4 and the shooting method to transform the boundary value problem into an initial value problem. The way the shooting method is implemented is with the newton method.
However, I don't get the right solutions.
As you can see in the following, the temperature (in red) is increasing as we are moving away from the plate whereas it should decrease exponentially.
It's more consistent for the fluid velocity (in blue), however the speed i think it should go up faster then go down faster. Here the curve is smoother.
Now, the fact is that we have a system of 2 coupled ODE. However, right now, I'm only trying to find the one of the two initials values (e.g. f''(0) = a, trying to find a) such that we have a solution to the boundary value problem (shooting method). Once found, I suppose we have the solution for the whole problem.
I guess I should maybe manage the two (f''(0) = a ; theta'(0) = b) but I don't know how to manage these two in parallel.
Last think to mention, if I try to get the initial value of theta' (so theta'(0)) I don't get the right heat profile.
Here is the code :
"""
The goal is to resolve a 3rd order non-linear ODE for the blasius problem.
It's made of 2 equations (flow / heat)
f''' = 3ff'' - 2(f')^2 + theta
3 Pr f theta' + theta'' = 0
RK4 + Shooting Method
"""
import numpy as np
import math
from scipy.integrate import odeint
from scipy.optimize import newton
from edo_solver.plot import plot
from constants import PRECISION
def blasius_edo(y, t, prandtl):
f = y[0:3]
theta = y[3:5]
return np.array([
# flow edo
f[1], # f' = df/dn
f[2], # f'' = d^2f/dn^2
- 3 * f[0] * f[2] + (2 * math.pow(f[1], 2)) - theta[0], # f''' = - 3ff'' + 2(f')^2 - theta,
# heat edo
theta[1], # theta' = dtheta/dn
- 3 * prandtl * f[0] * theta[1], # theta'' = - 3 Pr f theta'
])
def rk4(eta_range, shoot):
prandtl = 0.01
# initial values
f_init = [0, 0, shoot] # f(0), f'(0), f''(0)
theta_init = [1, shoot] # theta(0), theta'(0)
ci = f_init + theta_init # concatenate two ci
# note: tuple with single argument must have "," at the end of the tuple
return odeint(func=blasius_edo, y0=ci, t=eta_range, args=(prandtl,))
"""
if we have :
f'(t_0) = fprime_t0 ; f'(eta -> infty) = fprime_inf
we can transform it into :
f'(t_0) = fprime_t0 ; f''(t_0) = a
we define the function F(a) = f'(infty ; a) - fprime_inf
if F(a) has a root in "a",
then the solutions to the initial value problem with f''(t_0) = a
is also the solution the boundary problem with f'(eta -> infty) = fprime_inf
our goal is to find the root, we have the root...we have the solution.
it can be done with bissection method or newton method.
"""
def shooting(eta_range):
# boundary value
fprimeinf = 0 # f'(eta -> infty) = 0
# initial guess
# as far as I understand
# it has to be the good guess
# otherwise the result can be completely wrong
initial_guess = 10 # guess for f''(0)
# define our function to optimize
# our goal is to take big eta because eta should approach infty
# [-1, 1] : last row, second column => f'(eta_final) ~ f'(eta -> infty)
fun = lambda initial_guess: rk4(eta_range, initial_guess)[-1, 1] - fprimeinf
# newton method resolve the ODE system until eta_final
# then adjust the shoot and resolve again until we have a correct shoot
shoot = newton(func=fun, x0=initial_guess)
# resolve our system of ODE with the good "a"
y = rk4(eta_range, shoot)
return y
def compute_blasius_edo(title, eta_final):
ETA_0 = 0
ETA_INTERVAL = 0.1
ETA_FINAL = eta_final
# default values
title = title
x_label = "$\eta$"
y_label = "profil de vitesse $(f'(\eta))$ / profil de température $(\\theta)$"
legends = ["$f'(\eta)$", "$\\theta$"]
eta_range = np.arange(ETA_0, ETA_FINAL + ETA_INTERVAL, ETA_INTERVAL)
# shoot
y_set = shooting(eta_range)
plot(eta_range, y_set, title, legends, x_label, y_label)
compute_blasius_edo(
title="Convection naturelle - Solution de similitude",
eta_final=10
)

I could be completely off base here, but I wrote something similar to solve 1D fluid-reaction-heat equations. Try using solve_ivp and using the RADAU solver method, it helps with more difficult systems.
Also maybe try converting your system of ODES to a system of first order ODEs as that may help.

You are implementing the additional but wrong boundary condition f''(0) = theta'(0), as both slots get the same initial value in the shooting method. You need to hold them separate, giving 2 free variables and thus the need for a 2-dimensional Newton method or any other solver for non-scalar functions.
You could just as well use the solve_bvp routine with a sensible initial guess.

TDA - Sliding window on multiple signals

I'm trying to define a function in python that performs sliding window on multiple signals (and using the resulting SW as input for ripser).
What I want to achieve is this (examples with sines, sorry for bad drawing skills)
picture describing my goal
I have 14 signals of 10000 points, so a 14 x 10000 matrix, and I want to perform a sliding window on all the signals making them correlated in some way by grouping all the points for all the signals in each window, given its dimension.
I tried first using the code made by Christoper Tralie, but this gives me an error on the dimension of X, so now I'm trying to modify it.
def slidingWindowMultipleSignals(I, dim, Tau, dT):
'''
Performs the sliding window on multiple signals.
Author: Christopher J. Tralie
'''
N = I.shape[0] #Number of frames
P = I.shape[1] #Number of pixels (possibly after PCA)
pix = np.arange(P)
NWindows = int(np.floor((N-dim*Tau)/dT))
X = np.zeros((NWindows, dim*P))
idx = np.arange(N)
for i in range(NWindows):
idxx = dT*i + Tau*np.arange(dim)
start = int(np.floor(idxx[0]))
end = int(np.ceil(idxx[-1]))+2
if end >= I.shape[0]:
X = X[0:i, :]
break
f = scipy.interpolate.interp2d(pix, idx[start:end+1], I[idx[start:end+1], :], kind='linear')
X[i, :] = f(pix, idxx).flatten()
return X
The problem is that I don't know what to modify for making it doing the thing I described with the image.
Can someone point me to the right direction?

I suspect the problem is located in the line
NWindows = int(np.floor((N-dim*Tau)/dT))
specifically with the use of /. I'd check the dtypes of dim, Tau and dT. If all are integers, or if some are floats, / may not behave in exactly the same way.
Also, python expects the body of the function to be indented, which it isn't in your example.

How to reshape a networkx graph in Python?

So I created a really naive (probably inefficient) way of generating hasse diagrams.
Question:
I have 4 dimensions... p q r s .
I want to display it uniformly (tesseract) but I have no idea how to reshape it. How can one reshape a networkx graph in Python?
I've seen some examples of people using spring_layout() and draw_circular() but it doesn't shape in the way I'm looking for because they aren't uniform.
Is there a way to reshape my graph and make it uniform? (i.e. reshape my hasse diagram into a tesseract shape (preferably using nx.draw() )
Here's what mine currently look like:
Here's my code to generate the hasse diagram of N dimensions
#!/usr/bin/python
import networkx as nx
import matplotlib.pyplot as plt
import itertools
H = nx.DiGraph()
axis_labels = ['p','q','r','s']
D_len_node = {}
#Iterate through axis labels
for i in xrange(0,len(axis_labels)+1):
#Create edge from empty set
if i == 0:
for ax in axis_labels:
H.add_edge('O',ax)
else:
#Create all non-overlapping combinations
combinations = [c for c in itertools.combinations(axis_labels,i)]
D_len_node[i] = combinations
#Create edge from len(i-1) to len(i) #eg. pq >>> pqr, pq >>> pqs
if i > 1:
for node in D_len_node[i]:
for p_node in D_len_node[i-1]:
#if set.intersection(set(p_node),set(node)): Oops
if all(p in node for p in p_node) == True: #should be this!
H.add_edge(''.join(p_node),''.join(node))
#Show Plot
nx.draw(H,with_labels = True,node_shape = 'o')
plt.show()
I want to reshape it like this:
If anyone knows of an easier way to make Hasse Diagrams, please share some wisdom but that's not the main aim of this post.

This is a pragmatic, rather than purely mathematical answer.
I think you have two issues - one with layout, the other with your network.
1. Network
You have too many edges in your network for it to represent the unit tesseract. Caveat I'm not an expert on the maths here - just came to this from the plotting angle (matplotlib tag). Please explain if I'm wrong.
Your desired projection and, for instance, the wolfram mathworld page for a Hasse diagram for n=4 has only 4 edges connected all nodes, whereas you have 6 edges to the 2 and 7 edges to the 3 bit nodes. Your graph fully connects each "level", i.e. 4-D vectors with 0 1 values connect to all vectors with 1 1 value, which then connect to all vectors with 2 1 values and so on. This is most obvious in the projection based on the Wikipedia answer (2nd image below)
2. Projection
I couldn't find a pre-written algorithm or library to automatically project the 4D tesseract onto a 2D plane, but I did find a couple of examples, e.g. Wikipedia. From this, you can work out a co-ordinate set that would suit you and pass that into the nx.draw() call.
Here is an example - I've included two co-ordinate sets, one that looks like the projection you show above, one that matches this one from wikipedia.
import networkx as nx
import matplotlib.pyplot as plt
import itertools
H = nx.DiGraph()
axis_labels = ['p','q','r','s']
D_len_node = {}
#Iterate through axis labels
for i in xrange(0,len(axis_labels)+1):
#Create edge from empty set
if i == 0:
for ax in axis_labels:
H.add_edge('O',ax)
else:
#Create all non-overlapping combinations
combinations = [c for c in itertools.combinations(axis_labels,i)]
D_len_node[i] = combinations
#Create edge from len(i-1) to len(i) #eg. pq >>> pqr, pq >>> pqs
if i > 1:
for node in D_len_node[i]:
for p_node in D_len_node[i-1]:
if set.intersection(set(p_node),set(node)):
H.add_edge(''.join(p_node),''.join(node))
#This is manual two options to project tesseract onto 2D plane
# - many projections are available!!
wikipedia_projection_coords = [(0.5,0),(0.85,0.25),(0.625,0.25),(0.375,0.25),
(0.15,0.25),(1,0.5),(0.8,0.5),(0.6,0.5),
(0.4,0.5),(0.2,0.5),(0,0.5),(0.85,0.75),
(0.625,0.75),(0.375,0.75),(0.15,0.75),(0.5,1)]
#Build the "two cubes" type example projection co-ordinates
half_coords = [(0,0.15),(0,0.6),(0.3,0.15),(0.15,0),
(0.55,0.6),(0.3,0.6),(0.15,0.4),(0.55,1)]
#make the coords symmetric
example_projection_coords = half_coords + [(1-x,1-y) for (x,y) in half_coords][::-1]
print example_projection_coords
def powerset(s):
ch = itertools.chain.from_iterable(itertools.combinations(s, r) for r in range(len(s)+1))
return [''.join(t) for t in ch]
pos={}
for i,label in enumerate(powerset(axis_labels)):
if label == '':
label = 'O'
pos[label]= example_projection_coords[i]
#Show Plot
nx.draw(H,pos,with_labels = True,node_shape = 'o')
plt.show()
Note - unless you change what I've mentioned in 1. above, they still have your edge structure, so won't look exactly the same as the examples from the web. Here is what it looks like with your existing network generation code - you can see the extra edges if you compare it to your example (e.g. I don't this pr should be connected to pqs:
'Two cube' projection
Wikimedia example projection
Note
If you want to get into the maths of doing your own projections (and building up pos mathematically), you might look at this research paper.
EDIT:
Curiosity got the better of me and I had to search for a mathematical way to do this. I found this blog - the main result of which being the projection matrix:
This led me to develop this function for projecting each label, taking the label containing 'p' to mean the point has value 1 on the 'p' axis, i.e. we are dealing with the unit tesseract. Thus:
def construct_projection(label):
r1 = r2 = 0.5
theta = math.pi / 6
phi = math.pi / 3
x = int( 'p' in label) + r1 * math.cos(theta) * int('r' in label) - r2 * math.cos(phi) * int('s' in label)
y = int( 'q' in label) + r1 * math.sin(theta) * int('r' in label) + r2 * math.sin(phi) * int('s' in label)
return (x,y)
Gives a nice projection into a regular 2D octagon with all points distinct.
This will run in the above program, just replace
pos[label] = example_projection_coords[i]
with
pos[label] = construct_projection(label)
This gives the result:
play with r1,r2,theta and phi to your heart's content :)

How does Richardson–Lucy algorithm work? Code example?

I am trying to figure out how deconvolution works. I understand the idea behind it but I want to understand some of the actual algorithms which implement it - algorithms which take as input a blurred image with its point sample function (blur kernel) and produce as output the latent image.
So far I found Richardson–Lucy algorithm where the math does not seem to be that difficult however I can't figure how the actual algorithm works. At Wikipedia it says:
This leads to an equation for which can be solved iteratively according...
however it does not show the actual loop. Can anyone point me to a resource where the actual algorithm is explained. On Google I only manage to find methods which use Richardson–Lucy as one of its steps, but not the actual Richardson–Lucy algorithm.
Algorithm in any language or pseudo-code would be nice, however if one is available in Python, that would be amazing.
Thanx in advance.
Edit
Essentially what I want to figure out is given blurred image (nxm):
x00 x01 x02 x03 .. x0n
x10 x11 x12 x13 .. x1n
...
xm0 xm1 xm2 xm3 .. xmn
and the kernel (ixj) which was used in order to get the blurred image:
p00 p01 p02 .. p0i
p10 p11 p12 .. p1i
...
pj0 pj1 pj2 .. pji
What are the exact steps in the Richardson–Lucy algorithm in order to figure out the original image.

Here is a very simple Matlab implementation :
function result = RL_deconv(image, PSF, iterations)
% to utilise the conv2 function we must make sure the inputs are double
image = double(image);
PSF = double(PSF);
latent_est = image; % initial estimate, or 0.5*ones(size(image));
PSF_HAT = PSF(end:-1:1,end:-1:1); % spatially reversed psf
% iterate towards ML estimate for the latent image
for i= 1:iterations
est_conv = conv2(latent_est,PSF,'same');
relative_blur = image./est_conv;
error_est = conv2(relative_blur,PSF_HAT,'same');
latent_est = latent_est.* error_est;
end
result = latent_est;
original = im2double(imread('lena256.png'));
figure; imshow(original); title('Original Image')
hsize=[9 9]; sigma=1;
PSF = fspecial('gaussian', hsize, sigma);
blr = imfilter(original, PSF);
figure; imshow(blr); title('Blurred Image')
res_RL = RL_deconv(blr, PSF, 1000);
figure; imshow(res_RL); title('Recovered Image')
You can also work in the frequency domain instead of in the spatial domain as above. In that case the code would be :
function result = RL_deconv(image, PSF, iterations)
fn = image; % at the first iteration
OTF = psf2otf(PSF,size(image));
for i=1:iterations
ffn = fft2(fn);
Hfn = OTF.*ffn;
iHfn = ifft2(Hfn);
ratio = image./iHfn;
iratio = fft2(ratio);
res = OTF .* iratio;
ires = ifft2(res);
fn = ires.*fn;
end
result = abs(fn);
Only thing I don't quite understand is how this spatial reversal of the PSF works and what it's for. If anyone could explain that for me that would be cool! I'm also looking for a simple Matlab R-L implementation for spatially variant PSFs (ie spatially nonhomogeneous point spread functions) - if anyone would have one please let me know!
To get rid of the artefacts at the edges you could mirror the input image at the edges and then crop away the mirrored bits afterwards or use Matlab's image = edgetaper(image, PSF) before you call RL_deconv.
The native Matlab implementation deconvlucy.m is a bit more complicated btw - the source code of that one can be found here and uses an accelerated version of the basic algorithm.

The equation on Wikipedia gives a function for iteration t+1 in terms of iteration t. You can implement this type of iterative algorithm in the following way:
def iter_step(prev):
updated_value = <function from Wikipedia>
return updated_value
def iterate(initial_guess):
cur = initial_guess
while True:
prev, cur = cur, iter_step(cur)
if difference(prev, cur) <= tolerance:
break
return cur
Of course, you will have to implement your own difference function that is correct for whatever type of data you are working with. You also need to handle the case where convergence is never reached (e.g. limit the number of iterations).

Here's an open source Python implementation:
http://code.google.com/p/iocbio/wiki/IOCBioMicroscope

If it helps here is a implementation I wrote that includes some documentation....
https://github.com/bnorthan/projects/blob/master/truenorthJ/ImageJ2Plugins/functions/src/main/java/com/truenorth/functions/fft/filters/RichardsonLucyFilter.java
Richardson Lucy is a building block for many other deconvolution algorithms. For example the iocbio example above modified the algorithm to better deal with noise.
It is a relatively simple algorithm (as these things go) and is a starting point for more complicated algorithms so you can find many different implementations.