Convert a bivariate draw in a univariate draw in Matlab - algorithm

I have in mind the following experiment to run in Matlab and I am asking for an help to implement step (3). Any suggestion would be very appreciated.
(1) Consider the random variables X and Y both uniformly distributed on [0,1]
(2) Draw N realisation from the joint distribution of X and Y assuming that X and Y are independent (meaning that X and Y are uniformly jointly distributed on [0,1]x[0,1]). Each draw will be in [0,1]x[0,1].
(3) Transform each draw in [0,1]x[0,1] in a draw in [0,1] using the Hilbert space filling curve: under the Hilbert curve mapping, the draw in [0,1]x[0,1] should be the image of one (or more because of surjectivity) point(s) in [0,1]. I want pick one of these points. Is there any pre-built package in Matlab doing this?
I found this answer which I don't think does what I want as it explains how to obtain the Hilbert value of the draw (curve length from the start of curve to the picked point)
On wikipedia I found this code in C language (from (x,y) to d) which, again, does not fulfil my question.

EDIT This answer does not address updated version of the question, which explicitly asks about constructing Hilbert curve. Instead, this answer addresses a related question on construction of bijective mapping, and the relation to uniform distribution.
Your problem in not really well defined. If you only need the resulting distribution to be uniform, nothing is stopping you from simply picking f:(X,Y)->X. Result would be uniform regardless of whether X and Y are correlated. From your post I can only presume that what you want, in fact, is for the resulting transformation to be bijective, or as close to it as possible given machine precision limitations.
Worth noting that unless you need the algorithm that is best in preserving locality (which is clearly not required for resulting distribution to be bijective, not to mention uniform), there's no need to bother constructing Hilbert curves that you mention in your question. They have just as much to do with the solution as any other space-filling curve, and are incredibly computationally intensive.
So assuming you're looking for a bijective mapping, your question is equivalent to asking whether the set of points in a [unit] square has the same cardinality as the set of points in a [unit] line segment, and if it is, how to construct that bijection, i.e. 1-to-1 correspondence. The intuition says the square should have a higher cardinality, and Cantor spent 3 years trying to prove that, eventually proving quite the opposite - that these sets are in fact equinumerous. He was so surprised at his discovery that he wrote:
I see it, but I don't believe it!
The most commonly referred to bijection, fulfilling** this criteria, is the following. Represent x and y in their decimal form, i.e. x=0. x1 x2 x3 x4 x5..., and y=0. y1 y2 y3 y4 y5..., and let f:(X,Y)->Z be z=0. x1 y1 x2 y2 x3 y3 x4 y4 x5 y5..., i.e. alternating the decimals of the two numbers. The idea behind the bijection is trivial, though a rigorous proof requires quite a bit of prior knowledge.
** The caveat is that if we take e.g. x = 1/3 = 0.33333... and y = 1/5 = 0.199999... = 0.200000..., we can see there are two sequences corresponding to them: z = 0.313939393939... and z = 0.323030303030.... To overcome this obstacle we have to prove that adding a countable set to an uncountable one does not change the cardinality of the latter.
In reality we have to deal with machine precision and not pure math, which strictly speaking means both sets are actually finite and hence not equinumerous (assuming you store result with the same precision as original numbers). Which means we're simply forced to do some assumptions and loose some information, such as, in this case, the last half of significant digits of x and y. That is, unless we use a different data type that allows to store result with a double precision, compared to that of original variables.
Finally, sample implementation in Matlab:
x = rand();
y = rand();
chars = [num2str(x, '%.17f'); num2str(y, '%.17f')];
z = str2double(['0.' reshape(chars(:,3:end), 1, [])]);
>> cellstr(['x=' num2str(x, '%.17f'); 'y=' num2str(y, '%.17f'); 'z=' num2str(z, '%.17f')])
ans =
'x=0.65549803980353738'
'y=0.10975505072305158'
'z=0.61505947958500362'

Edit This answers the original request for a transformation f(x,y) -> t ~ U[0,1] given x,y ~ U[0,1], and additionally for x and y correlated. The updated question asks specifically for a Hilbert curve, H(x,y) -> t ~ U[0,1] and only for x,y ~ U[0,1] so this answer is no longer relevant.
Consider a random uniform sequence in [0,1] r1, r2, r3, .... You are assigning this sequence to pairs of numbers (x1,y1), (x2,y2), .... What you are asking for is a transformation on pairs (x,y) which yield a uniform random number in [0,1].
Consider the random subsequence r1, r3, ... corresponding to x1, x2, .... If you trust that your number generator is random and uncorrelated in [0,1], then the subsequence x1, x2, ... should also be random and uncorrelated in [0,1]. So the rather simple answer to the first part of your question is a projection onto either the x or y axis. That is, just pick x.
Next consider correlations between x and y. Since you haven't specified the nature of the correlation, let's assume a simple scaling of the axes,
such as x' => [0, 0.5], y' => [0, 3.0], followed by a rotation. The scaling doesn't introduce any correlation since x' and y' are still independent. You can generate it easily enough with a matrix multiply:
M1*p = [x_scale, 0; 0, y_scale] * [x; y]
for matrix M1 and point p. You can introduce a correlation by taking this stretched form and rotating it by theta:
M2*M1*p = [cos(theta), sin(theta); -sin(theta), cos(theta)]*M1*p
Putting it all together with theta = pi/4, or 45 degrees, you can see that larger values of y are correlated with larger values of x:
cos_t = sin_t = cos(pi/4); % at 45 degrees, sin(t) = cos(t) = 1/sqrt(2)
M2 = [cos_t, sin_t; -sin_t, cos_t];
M1 = [0.5, 0.0; 0.0, 3.0];
p = random(2,1000);
p_prime = M2*M1*p;
plot(p_prime(1)', p_prime(2)', '.');
axis('equal');
The resulting plot* shows a band of uniformly distributed numbers at a 45 degree angle:
Further transformations are possible with shear, and if you are clever about it, translation (OpenGL uses 4x4 transformation matrices so that translation can be represented as a linear transform matrix, with an extra dimension added before the transformation steps and removed before they are done).
Given a known affine correlation structure, you can transform back from random points (x',y') to points (x,y) where x and y are independent in [0,1] by solving Mk*...*M1 p = p_prime for p, or equivalently, by setting p = inv(Mk*...*M1) * p_prime, where p=[x;y]. Again, just pick x, which will be uniform in [0,1]. This doesn't work if the transformation matrix is singular, e.g., if you introduce a projection matrix Mj into the mix (though if the projection is the first step you can still recover).
* You may notice that the plot is from python rather than matlab. I don't have matlab or octave sitting in front of me right now, so I hope I got the syntax details right.

You could compute the hilbert curve from f(x,y)=z. Basically it's a hamiltonian path traversal. You can find a good description at Nick's spatial index hilbert curve quadtree blog. Or take a look at monotonic n-ary gray code. I've written an implementation based on Nick's blog in php:http://monstercurves.codeplex.com.

I will focus only on your last point
(3) Transform each draw in [0,1]x[0,1] in a draw in [0,1] using the Hilbert space filling curve: under the Hilbert curve mapping, the draw in [0,1]x[0,1] should be the image of one (or more because of surjectivity) point(s) in [0,1]. I want pick one of these points. Is there any pre-built package in Matlab doing this?
As far as I know, there aren't pre-built packages in Matlab doing this, but the good news is that the code on wikipedia can be called from MATLAB, and it is as simple as putting together the conversion routine with a gateway function in a xy2d.c file:
#include "mex.h"
// source: https://en.wikipedia.org/wiki/Hilbert_curve
// rotate/flip a quadrant appropriately
void rot(int n, int *x, int *y, int rx, int ry) {
if (ry == 0) {
if (rx == 1) {
*x = n-1 - *x;
*y = n-1 - *y;
}
//Swap x and y
int t = *x;
*x = *y;
*y = t;
}
}
// convert (x,y) to d
int xy2d (int n, int x, int y) {
int rx, ry, s, d=0;
for (s=n/2; s>0; s/=2) {
rx = (x & s) > 0;
ry = (y & s) > 0;
d += s * s * ((3 * rx) ^ ry);
rot(s, &x, &y, rx, ry);
}
return d;
}
/* The gateway function */
void mexFunction( int nlhs, mxArray *plhs[],
int nrhs, const mxArray *prhs[])
{
int n; /* input scalar */
int x; /* input scalar */
int y; /* input scalar */
int *d; /* output scalar */
/* check for proper number of arguments */
if(nrhs!=3) {
mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs","Three inputs required.");
}
if(nlhs!=1) {
mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nlhs","One output required.");
}
/* get the value of the scalar inputs */
n = mxGetScalar(prhs[0]);
x = mxGetScalar(prhs[1]);
y = mxGetScalar(prhs[2]);
/* create the output */
plhs[0] = mxCreateDoubleScalar(xy2d(n,x,y));
/* get a pointer to the output scalar */
d = mxGetPr(plhs[0]);
}
and compile it with mex('xy2d.c').
The above implementation
[...] assumes a square divided into n by n cells, for n a power of 2, with integer coordinates, with (0,0) in the lower left corner, (n-1,n-1) in the upper right corner.
In practice, a discretization step is required before applying the mapping. As in every discretization problem, it is crucial to choose the precision wisely. The snippet below puts everything together.
close all; clear; clc;
% number of random samples
NSAMPL = 100;
% unit square divided into n-by-n cells
% has to be a power of 2
n = 2^2;
% quantum
d = 1/n;
N = 0:d:1;
% generate random samples
x = rand(1,NSAMPL);
y = rand(1,NSAMPL);
% discretization
bX = floor(x/d);
bY = floor(y/d);
% 2d to 1d mapping
dd = zeros(1,NSAMPL);
for iid = 1:length(dd)
dd(iid) = xy2d(n, bX(iid), bY(iid));
end
figure;
hold on;
axis equal;
plot(x, y, '.');
plot(repmat([0;1], 1, length(N)), repmat(N, 2, 1), '-r');
plot(repmat(N, 2, 1), repmat([0;1], 1, length(N)), '-r');
figure;
plot(1:NSAMPL, dd);
xlabel('# of sample')

Related

Smallest enclosing regular hexagon

Is there any algorithm / method to find the smallest regular hexagon around a set of points (x, y).
And by smallest I mean smallest area.
My current idea was to find the smallest circle enclosing the points, and then create a hexagon from there and check if all the points are inside, but that is starting to sound like a never ending problem.
Requirements
First of all, let's define a hexagon as quadruple [x0, y0, t0, s], where (x0, y0), t0 and s are its center, rotation and side-length respectively.
Next, we need to find whether an arbitrary point is inside the hexagon. The following functions do this:
function getHexAlpha(t, hex)
t = t - hex.t0;
t = t - 2*pi * floor(t / (2*pi));
return pi/2 - abs(rem(t, pi/3) - (pi/6));
end
function getHexRadious( P, hex )
x = P.x - hex.x0;
y = P.y - hex.y0;
t = atan2(y, x);
return hex.s * cos(pi/6) / sin(getHexAlpha(t, hex));
end
function isInHex(P, hex)
r = getHexRadious(P, hex);
d = sqrt((P.x - hex.x0)^2 + (P.y - hex.y0)^2);
return r >= d;
end
Long story short, the getHexRadious function formulates the hexagon in polar form and returns distance from center of hexagon to its boundary at each angle. Read this post for more details about getHexRadious and getHexRadious functions. This is how these work for a set of random points and an arbitrary hexagon:
The Algorithm
I suggest a two-stepped algorithm:
1- Guess an initial hexagon that covers most of points :)
2- Tune s to cover all points
Chapter 1: (2) Following Tarantino in Kill Bill Vol.1
For now, let's assume that our arbitrary hexagon is a good guess. Following functions keep x0, y0, t0 and tune s to cover all points:
function getHexSide( P, hex )
x = P.x - hex.x0;
y = P.y - hex.y0;
r = sqrt(x^2 + y^2);
t = atan2(y, x);
return r / (cos(pi/6) / sin(getHexAlpha(t, hex)));
end
function findMinSide( P[], hex )
for all P[i] in P
S[i] = getHexSide(P, hex);
end
return max(S[]);
end
The getHexSide function is reverse of getHexRadious. It returns the minimum required side-length for a hexagon with x0, y0, t0 to cover point P. This is the outcome for previous test case:
Chapter 2: (1)
As a guess, we can find two points furthest away from each other and fit one of hexagon diameters' on them:
function guessHex( P[] )
D[,] = pairwiseDistance(P[]);
[i, j] = indexOf(max(max(D[,])));
[~, j] = max(D(i, :));
hex.x0 = (P[i].x + P[j].x) / 2;
hex.y0 = (P[i].y + P[j].y) / 2;
hex.s = D[i, j]/2;
hex.t0 = atan2(P.y(i)-hex.y0, P.x(i)-hex.x0);
return hex;
end
Although this method can find a relatively small polygon, but as a greedy approach, it never guarantees to find the optimum solutions.
Chapter 3: A Better Guess
Well, this problem is definitely an optimization problem with its objective being to minimize area of hexagon (or s variable). I don't know if it has an analytical solution, and SO is not the right place to discuss it. But any optimization algorithm can be used to provide a better initial guess. I used GA to solve this with findMinSide as its cost function. In fact GA generates many guesses about x0, y0, and t0 and the best one will be selected. It finds better results but is more time consuming. Still no guarantee to find the optimum!
Optimization of Optimization
When it comes to optimization algorithms, performance is always an issue. Keep in mind that hexagon only needs to enclose the convex-hall of points. If you are dealing with large sets of points, it's better to find the convex-hall and get rid of the rest of the points.

Check if point inside random boxes

Edit: I've worked a solution. Feel free to contact me if you come across this in the future and need something similar.
--
Instead of generating random points on a plane, how would you check if a given coordinate is equal to a random point? Or inside a random bounding box?
For example you have a plane with integer coordinates. That plane is somehow populated with random bounding boxes (generated using a formula, not data). The goal is to check if a given (x, y) is within one of those boxes.
I can find many references on how to generate random points but not much for doing it in this more backwards way (I guess you'd call it 'functional'?).
I have managed to make an algorithm that splits the plane into 100x100 squares, and within each square is a bounding box that is randomly placed. But is it possible with an algorithm that places the boxes more organically?
Edit: Here's an example algorithm I used for a simple "random point within a 100x100 grid" (from memory, might be missing something):
// check if equal to a random point within the point's grid square
boolean isRandomCenter(x, y) {
// offset relative to origin of grid square
int offsetX = x mod 100
int offsetY = y mod 100
// any random seed will do
int randomSeed = x * y
// random position of point for this square
int randomOffsetX = random(50, randomSeed)
int randomOffsetY = random(50, randomSeed)
if (offsetX == randomOffsetX && offsetY == randomOFfsetY)
return true
return false
}
Well, I don't know if I exactly understand your problem, but the condition to know if a given point M(x, y) plotted in a 2 dimensional Euclidian space represented with two axes x and y is inside a box represented with two opposites points A(xa, ya) and B(xb, yb) is pretty simple.
Let's define a function isInsideTheBox(x, y, xa, ya, xb, yb) returning true if M is inside the box and false else :
bool isInsideTheBox(int x, int y, int xa, int ya, int xb, int yb)
{
// We assume xa < xb and ya < yb
return (x >= xa && x <= xb && y >= ya && y <= yb);
}
I am answering the question: check if a point is over a random point.
If the coordinates are real, the probability of an overlap is null and the question is virtually useless. So I assume discrete coordinates.
If the question regards random points that have already been drawn, the only way is to remember the random points in some container as you draw them (array, sorted, list, search tree, hash table).
If the question regards points that might be drawn at that location, the answer is "true" in the whole domain (where the distribution is nonzero). You need to model the domain geometrically to perform point-in-... queries.
If the question is about pseudo-random or quasi-random points, I don't think there is any shortcut and you should proceed as for the truly random case (unless the generator is really poor).

Curve (spline, bezier path, etc) described by a set of base points

As an input I have set of 'base' points (e.g. 9 points), and as an output I must return another set of points, which describe a curve.
A1-A9 is an input; these are the 'base' points. My task is to return a set of points, from which the user can build the depicted curve, the black line from A1-A9
My mathematical skills are low, and Googling is not very helpful. As I understand, this can be a cubic spline. I have found some C-based source code, but this code loops endlessly when I try to build spline parts, where nextPoint.x < currentPoint.x.
Please, explain me, what kind of splines, bezier paths, or other constructs I should use for my task. It will be very good if you point me to code, an algorithm, or a good manual for dummies.
Use Interpolation methods to generate the intermediate points on your curve.
For example, given the CubicInterpolate function:
double CubicInterpolate(
double y0,double y1,
double y2,double y3,
double mu)
{
double a0,a1,a2,a3,mu2;
mu2 = mu*mu;
a0 = y3 - y2 - y0 + y1;
a1 = y0 - y1 - a0;
a2 = y2 - y0;
a3 = y1;
return(a0*mu*mu2+a1*mu2+a2*mu+a3);
}
to find the point halfway between point[1] and point[2] on a cubic spline, you would use:
newPoint.X = CubicInterpolate(point[0].X, point[1].X, point[2].X, point[3].X, 0.5);
newPoint.Y = CubicInterpolate(point[0].Y, point[1].Y, point[2].Y, point[3].Y, 0.5);
point[0] and point[3] do affect the section of the curve between point[1] and point[2]. At either end of the curve, simply use the end point again.
To ensure a roughly equal distance between points, you can calculate the distance between input points to determine how many intermediate points (and mu values) to generate. So, for points that are further apart, you would use many more mu values between 0 and 1. Conversely, for points that are very close together, you may not need to add intermediate points at all.
Thank you all. I found the solution. For build 2D curve by basic points I do follow:
I found this article about a cubic spline with C++ and C# examples. This example allows to find interpolation values of 'one dimension' cubic spline by base points. Because I need a two dimension cubic spline - I create two one dimension splines - for 'x' and 'y' axes. Next, Next, I was run a cycle from first point to end point with some step and in each iteration of cycle I found interpolation value. From interpolation value I was make a point. So, when cycle has be ended I get a curve.
pseudo code (using spline class from article pointed above):
- (array*)splineByBasePoints:(array*)basePoints
{
int n = basePoints.count;
cubic_spline xSpline, ySpline;
xSpline.build_spline(basePoints.pointNumbers, basePoints.XValuesOfPoints, n);
ySpline.build_spline(basePoints.pointNumbers, basePoints.YValuesOfPoints, n);
array curve;
int t = 1; //t - intermediate point. '1' because number of point, not index
for (; t <= n; t += step)
{
[curve addToArray:PontWithXY([xSpline f:t], [ySpline f:t])];
}
return array;
}
If you have MATLAB license,
x = -4:4;
y = [0 .15 1.12 2.36 2.36 1.46 .49 .06 0];
cs = spline(x,[0 y 0]);
xx = linspace(-4,4,101);
y=ppval(cs,xx);

Calculating quaternion for transformation between 2 3D cartesian coordinate systems

I have two cartesian coordinate systems with known unit vectors:
System A(x_A,y_A,z_A)
and
System B(x_B,y_B,z_B)
Both systems share the same origin (0,0,0). I'm trying to calculate a quaternion, so that vectors in system B can be expressed in system A.
I am familiar with the mathematical concept of quaternions. I have already implemented the required math from here: http://content.gpwiki.org/index.php/OpenGL%3aTutorials%3aUsing_Quaternions_to_represent_rotation
One possible solution could be to calculate Euler angles and use them for 3 quaternions. Multiplying them would lead to a final one, so that I could transform my vectors:
v(A) = q*v(B)*q_conj
But this would incorporate Gimbal Lock again, which was the reason NOT to use Euler angles in the beginning.
Any idead how to solve this?
You can calculate the quaternion representing the best possible transformation from one coordinate system to another by the method described in this paper:
Paul J. Besl and Neil D. McKay
"Method for registration of 3-D shapes", Sensor Fusion IV: Control Paradigms and Data Structures, 586 (April 30, 1992); http://dx.doi.org/10.1117/12.57955
The paper is not open access but I can show you the Python implementation:
def get_quaternion(lst1,lst2,matchlist=None):
if not matchlist:
matchlist=range(len(lst1))
M=np.matrix([[0,0,0],[0,0,0],[0,0,0]])
for i,coord1 in enumerate(lst1):
x=np.matrix(np.outer(coord1,lst2[matchlist[i]]))
M=M+x
N11=float(M[0][:,0]+M[1][:,1]+M[2][:,2])
N22=float(M[0][:,0]-M[1][:,1]-M[2][:,2])
N33=float(-M[0][:,0]+M[1][:,1]-M[2][:,2])
N44=float(-M[0][:,0]-M[1][:,1]+M[2][:,2])
N12=float(M[1][:,2]-M[2][:,1])
N13=float(M[2][:,0]-M[0][:,2])
N14=float(M[0][:,1]-M[1][:,0])
N21=float(N12)
N23=float(M[0][:,1]+M[1][:,0])
N24=float(M[2][:,0]+M[0][:,2])
N31=float(N13)
N32=float(N23)
N34=float(M[1][:,2]+M[2][:,1])
N41=float(N14)
N42=float(N24)
N43=float(N34)
N=np.matrix([[N11,N12,N13,N14],\
[N21,N22,N23,N24],\
[N31,N32,N33,N34],\
[N41,N42,N43,N44]])
values,vectors=np.linalg.eig(N)
w=list(values)
mw=max(w)
quat= vectors[:,w.index(mw)]
quat=np.array(quat).reshape(-1,).tolist()
return quat
This function returns the quaternion that you were looking for. The arguments lst1 and lst2 are lists of numpy.arrays where every array represents a 3D vector. If both lists are of length 3 (and contain orthogonal unit vectors), the quaternion should be the exact transformation. If you provide longer lists, you get the quaternion that is minimizing the difference between both point sets.
The optional matchlist argument is used to tell the function which point of lst2 should be transformed to which point in lst1. If no matchlist is provided, the function assumes that the first point in lst1 should match the first point in lst2 and so forth...
A similar function for sets of 3 Points in C++ is the following:
#include <Eigen/Dense>
#include <Eigen/Geometry>
using namespace Eigen;
/// Determine rotation quaternion from coordinate system 1 (vectors
/// x1, y1, z1) to coordinate system 2 (vectors x2, y2, z2)
Quaterniond QuaternionRot(Vector3d x1, Vector3d y1, Vector3d z1,
Vector3d x2, Vector3d y2, Vector3d z2) {
Matrix3d M = x1*x2.transpose() + y1*y2.transpose() + z1*z2.transpose();
Matrix4d N;
N << M(0,0)+M(1,1)+M(2,2) ,M(1,2)-M(2,1) , M(2,0)-M(0,2) , M(0,1)-M(1,0),
M(1,2)-M(2,1) ,M(0,0)-M(1,1)-M(2,2) , M(0,1)+M(1,0) , M(2,0)+M(0,2),
M(2,0)-M(0,2) ,M(0,1)+M(1,0) ,-M(0,0)+M(1,1)-M(2,2) , M(1,2)+M(2,1),
M(0,1)-M(1,0) ,M(2,0)+M(0,2) , M(1,2)+M(2,1) ,-M(0,0)-M(1,1)+M(2,2);
EigenSolver<Matrix4d> N_es(N);
Vector4d::Index maxIndex;
N_es.eigenvalues().real().maxCoeff(&maxIndex);
Vector4d ev_max = N_es.eigenvectors().col(maxIndex).real();
Quaterniond quat(ev_max(0), ev_max(1), ev_max(2), ev_max(3));
quat.normalize();
return quat;
}
What language are you using? If c++, feel free to use my open source library:
http://sourceforge.net/p/transengine/code/HEAD/tree/transQuaternion/
The short of it is, you'll need to convert your vectors to quaternions, do your calculations, and then convert your quaternion to a transformation matrix.
Here's a code snippet:
Quaternion from vector:
cQuat nTrans::quatFromVec( Vec vec ) {
float angle = vec.v[3];
float s_angle = sin( angle / 2);
float c_angle = cos( angle / 2);
return (cQuat( c_angle, vec.v[0]*s_angle, vec.v[1]*s_angle,
vec.v[2]*s_angle )).normalized();
}
And for the matrix from quaternion:
Matrix nTrans::matFromQuat( cQuat q ) {
Matrix t;
q = q.normalized();
t.M[0][0] = ( 1 - (2*q.y*q.y + 2*q.z*q.z) );
t.M[0][1] = ( 2*q.x*q.y + 2*q.w*q.z);
t.M[0][2] = ( 2*q.x*q.z - 2*q.w*q.y);
t.M[0][3] = 0;
t.M[1][0] = ( 2*q.x*q.y - 2*q.w*q.z);
t.M[1][1] = ( 1 - (2*q.x*q.x + 2*q.z*q.z) );
t.M[1][2] = ( 2*q.y*q.z + 2*q.w*q.x);
t.M[1][3] = 0;
t.M[2][0] = ( 2*q.x*q.z + 2*q.w*q.y);
t.M[2][1] = ( 2*q.y*q.z - 2*q.w*q.x);
t.M[2][2] = ( 1 - (2*q.x*q.x + 2*q.y*q.y) );
t.M[2][3] = 0;
t.M[3][0] = 0;
t.M[3][1] = 0;
t.M[3][2] = 0;
t.M[3][3] = 1;
return t;
}
I just ran into this same problem. I was on the track to a solution, but I got stuck.
So, you'll need TWO vectors which are known in both coordinate systems. In my case, I have 2 orthonormal vectors in the coordinate system of a device (gravity and magnetic field), and I want to find the quaternion to rotate from device coordinates to global orientation (where North is positive Y, and "up" is positive Z). So, in my case, I've measured the vectors in the device coordinate space, and I'm defining the vectors themselves to form the orthonormal basis for the global system.
With that said, consider the axis-angle interpretation of quaternions, there is some vector V about which the device's coordinates can be rotated by some angle to match the global coordinates. I'll call my (negative) gravity vector G, and magnetic field M (both are normalized).
V, G and M all describe points on the unit sphere.
So do Z_dev and Y_dev (the Z and Y bases for my device's coordinate system).
The goal is to find a rotation which maps G onto Z_dev and M onto Y_dev.
For V to rotate G onto Z_dev the distance between the points defined by G and V must be the same as the distance between the points defined by V and Z_dev. In equations:
|V - G| = |V - Z_dev|
The solution to this equation forms a plane (all points equidistant to G and Z_dev). But, V is constrained to be unit-length, which means the solution is a ring centered on the origin -- still an infinite number of points.
But, the same situation is true of Y_dev, M and V:
|V - M| = |V - Y_dev|
The solution to this is also a ring centered on the origin. These rings have two intersection points, where one is the negative of the other. Either is a valid axis of rotation (the angle of rotation will just be negative in one case).
Using the two equations above, and the fact that each of these vectors is unit length you should be able to solve for V.
Then you just have to find the angle to rotate by, which you should be able to do using the vectors going from V to your corresponding bases (G and Z_dev for me).
Ultimately, I got gummed up towards the end of the algebra in solving for V.. but either way, I think everything you need is here -- maybe you'll have better luck than I did.
Define 3x3 matrices A and B as you gave them, so the columns of A are x_A,x_B, and x_C and the columns of B are similarly defined. Then the transformation T taking coordinate system A to B is the solution TA = B, so T = BA^{-1}. From the rotation matrix T of the transformation you can calculate the quaternion using standard methods.
You need to express the orientation of B, with respect to A as a quaternion Q. Then any vector in B can be transformed to a vector in A e.g. by using a rotation matrix R derived from Q. vectorInA = R*vectorInB.
There is a demo script for doing this (including a nice visualization) in the Matlab/Octave library available on this site: http://simonbox.info/index.php/blog/86-rocket-news/92-quaternions-to-model-rotations
You can compute what you want using only quaternion algebra.
Given two unit vectors v1 and v2 you can directly embed them into quaternion algebra and get the corresponding pure quaternions q1 and q2. The rotation quaternion Q that align the two vectors such that:
Q q1 Q* = q2
is given by:
Q = q1 (q1 + q2)/(||q1 + q2||)
The above product is the quaternion product.

3D Least Squares Plane

What's the algorithm for computing a least squares plane in (x, y, z) space, given a set of 3D data points? In other words, if I had a bunch of points like (1, 2, 3), (4, 5, 6), (7, 8, 9), etc., how would one go about calculating the best fit plane f(x, y) = ax + by + c? What's the algorithm for getting a, b, and c out of a set of 3D points?
If you have n data points (x[i], y[i], z[i]), compute the 3x3 symmetric matrix A whose entries are:
sum_i x[i]*x[i], sum_i x[i]*y[i], sum_i x[i]
sum_i x[i]*y[i], sum_i y[i]*y[i], sum_i y[i]
sum_i x[i], sum_i y[i], n
Also compute the 3 element vector b:
{sum_i x[i]*z[i], sum_i y[i]*z[i], sum_i z[i]}
Then solve Ax = b for the given A and b. The three components of the solution vector are the coefficients to the least-square fit plane {a,b,c}.
Note that this is the "ordinary least squares" fit, which is appropriate only when z is expected to be a linear function of x and y. If you are looking more generally for a "best fit plane" in 3-space, you may want to learn about "geometric" least squares.
Note also that this will fail if your points are in a line, as your example points are.
The equation for a plane is: ax + by + c = z. So set up matrices like this with all your data:
x_0 y_0 1
A = x_1 y_1 1
...
x_n y_n 1
And
a
x = b
c
And
z_0
B = z_1
...
z_n
In other words: Ax = B. Now solve for x which are your coefficients. But since (I assume) you have more than 3 points, the system is over-determined so you need to use the left pseudo inverse. So the answer is:
a
b = (A^T A)^-1 A^T B
c
And here is some simple Python code with an example:
import matplotlib.pyplot as plt
from mpl_toolkits.mplot3d import Axes3D
import numpy as np
N_POINTS = 10
TARGET_X_SLOPE = 2
TARGET_y_SLOPE = 3
TARGET_OFFSET = 5
EXTENTS = 5
NOISE = 5
# create random data
xs = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
ys = [np.random.uniform(2*EXTENTS)-EXTENTS for i in range(N_POINTS)]
zs = []
for i in range(N_POINTS):
zs.append(xs[i]*TARGET_X_SLOPE + \
ys[i]*TARGET_y_SLOPE + \
TARGET_OFFSET + np.random.normal(scale=NOISE))
# plot raw data
plt.figure()
ax = plt.subplot(111, projection='3d')
ax.scatter(xs, ys, zs, color='b')
# do fit
tmp_A = []
tmp_b = []
for i in range(len(xs)):
tmp_A.append([xs[i], ys[i], 1])
tmp_b.append(zs[i])
b = np.matrix(tmp_b).T
A = np.matrix(tmp_A)
fit = (A.T * A).I * A.T * b
errors = b - A * fit
residual = np.linalg.norm(errors)
print("solution:")
print("%f x + %f y + %f = z" % (fit[0], fit[1], fit[2]))
print("errors:")
print(errors)
print("residual:")
print(residual)
# plot plane
xlim = ax.get_xlim()
ylim = ax.get_ylim()
X,Y = np.meshgrid(np.arange(xlim[0], xlim[1]),
np.arange(ylim[0], ylim[1]))
Z = np.zeros(X.shape)
for r in range(X.shape[0]):
for c in range(X.shape[1]):
Z[r,c] = fit[0] * X[r,c] + fit[1] * Y[r,c] + fit[2]
ax.plot_wireframe(X,Y,Z, color='k')
ax.set_xlabel('x')
ax.set_ylabel('y')
ax.set_zlabel('z')
plt.show()
unless someone tells me how to type equations here, let me just write down the final computations you have to do:
first, given points r_i \n \R, i=1..N, calculate the center of mass of all points:
r_G = \frac{\sum_{i=1}^N r_i}{N}
then, calculate the normal vector n, that together with the base vector r_G defines the plane by calculating the 3x3 matrix A as
A = \sum_{i=1}^N (r_i - r_G)(r_i - r_G)^T
with this matrix, the normal vector n is now given by the eigenvector of A corresponding to the minimal eigenvalue of A.
To find out about the eigenvector/eigenvalue pairs, use any linear algebra library of your choice.
This solution is based on the Rayleight-Ritz Theorem for the Hermitian matrix A.
See 'Least Squares Fitting of Data' by David Eberly for how I came up with this one to minimize the geometric fit (orthogonal distance from points to the plane).
bool Geom_utils::Fit_plane_direct(const arma::mat& pts_in, Plane& plane_out)
{
bool success(false);
int K(pts_in.n_cols);
if(pts_in.n_rows == 3 && K > 2) // check for bad sizing and indeterminate case
{
plane_out._p_3 = (1.0/static_cast<double>(K))*arma::sum(pts_in,1);
arma::mat A(pts_in);
A.each_col() -= plane_out._p_3; //[x1-p, x2-p, ..., xk-p]
arma::mat33 M(A*A.t());
arma::vec3 D;
arma::mat33 V;
if(arma::eig_sym(D,V,M))
{
// diagonalization succeeded
plane_out._n_3 = V.col(0); // in ascending order by default
if(plane_out._n_3(2) < 0)
{
plane_out._n_3 = -plane_out._n_3; // upward pointing
}
success = true;
}
}
return success;
}
Timed at 37 micro seconds fitting a plane to 1000 points (Windows 7, i7, 32bit program)
This reduces to the Total Least Squares problem, that can be solved using SVD decomposition.
C++ code using OpenCV:
float fitPlaneToSetOfPoints(const std::vector<cv::Point3f> &pts, cv::Point3f &p0, cv::Vec3f &nml) {
const int SCALAR_TYPE = CV_32F;
typedef float ScalarType;
// Calculate centroid
p0 = cv::Point3f(0,0,0);
for (int i = 0; i < pts.size(); ++i)
p0 = p0 + conv<cv::Vec3f>(pts[i]);
p0 *= 1.0/pts.size();
// Compose data matrix subtracting the centroid from each point
cv::Mat Q(pts.size(), 3, SCALAR_TYPE);
for (int i = 0; i < pts.size(); ++i) {
Q.at<ScalarType>(i,0) = pts[i].x - p0.x;
Q.at<ScalarType>(i,1) = pts[i].y - p0.y;
Q.at<ScalarType>(i,2) = pts[i].z - p0.z;
}
// Compute SVD decomposition and the Total Least Squares solution, which is the eigenvector corresponding to the least eigenvalue
cv::SVD svd(Q, cv::SVD::MODIFY_A|cv::SVD::FULL_UV);
nml = svd.vt.row(2);
// Calculate the actual RMS error
float err = 0;
for (int i = 0; i < pts.size(); ++i)
err += powf(nml.dot(pts[i] - p0), 2);
err = sqrtf(err / pts.size());
return err;
}
As with any least-squares approach, you proceed like this:
Before you start coding
Write down an equation for a plane in some parameterization, say 0 = ax + by + z + d in thee parameters (a, b, d).
Find an expression D(\vec{v};a, b, d) for the distance from an arbitrary point \vec{v}.
Write down the sum S = \sigma_i=0,n D^2(\vec{x}_i), and simplify until it is expressed in terms of simple sums of the components of v like \sigma v_x, \sigma v_y^2, \sigma v_x*v_z ...
Write down the per parameter minimization expressions dS/dx_0 = 0, dS/dy_0 = 0 ... which gives you a set of three equations in three parameters and the sums from the previous step.
Solve this set of equations for the parameters.
(or for simple cases, just look up the form). Using a symbolic algebra package (like Mathematica) could make you life much easier.
The coding
Write code to form the needed sums and find the parameters from the last set above.
Alternatives
Note that if you actually had only three points, you'd be better just finding the plane that goes through them.
Also, if the analytic solution in unfeasible (not the case for a plane, but possible in general) you can do steps 1 and 2, and use a Monte Carlo minimizer on the sum in step 3.
CGAL::linear_least_squares_fitting_3
Function linear_least_squares_fitting_3 computes the best fitting 3D
line or plane (in the least squares sense) of a set of 3D objects such
as points, segments, triangles, spheres, balls, cuboids or tetrahedra.
http://www.cgal.org/Manual/latest/doc_html/cgal_manual/Principal_component_analysis_ref/Function_linear_least_squares_fitting_3.html
It sounds like all you want to do is linear regression with 2 regressors. The wikipedia page on the subject should tell you all you need to know and then some.
All you'll have to do is to solve the system of equations.
If those are your points:
(1, 2, 3), (4, 5, 6), (7, 8, 9)
That gives you the equations:
3=a*1 + b*2 + c
6=a*4 + b*5 + c
9=a*7 + b*8 + c
So your question actually should be: How do I solve a system of equations?
Therefore I recommend reading this SO question.
If I've misunderstood your question let us know.
EDIT:
Ignore my answer as you probably meant something else.
We first present a linear least-squares plane fitting method that minimizes the residuals between the estimated normal vector and provided points.
Recall that the equation for a plane passing through origin is Ax + By + Cz = 0, where (x, y, z) can be any point on the plane and (A, B, C) is the normal vector perpendicular to this plane.
The equation for a general plane (that may or may not pass through origin) is Ax + By + Cz + D = 0, where the additional coefficient D represents how far the plane is away from the origin, along the direction of the normal vector of the plane. [Note that in this equation (A, B, C) forms a unit normal vector.]
Now, we can apply a trick here and fit the plane using only provided point coordinates. Divide both sides by D and rearrange this term to the right-hand side. This leads to A/D x + B/D y + C/D z = -1. [Note that in this equation (A/D, B/D, C/D) forms a normal vector with length 1/D.]
We can set up a system of linear equations accordingly, and then solve it by an Eigen solver in C++ as follows.
// Example for 5 points
Eigen::Matrix<double, 5, 3> matA; // row: 5 points; column: xyz coordinates
Eigen::Matrix<double, 5, 1> matB = -1 * Eigen::Matrix<double, 5, 1>::Ones();
// Find the plane normal
Eigen::Vector3d normal = matA.colPivHouseholderQr().solve(matB);
// Check if the fitting is healthy
double D = 1 / normal.norm();
normal.normalize(); // normal is a unit vector from now on
bool planeValid = true;
for (int i = 0; i < 5; ++i) { // compare Ax + By + Cz + D with 0.2 (ideally Ax + By + Cz + D = 0)
if ( fabs( normal(0)*matA(i, 0) + normal(1)*matA(i, 1) + normal(2)*matA(i, 2) + D) > 0.2) {
planeValid = false; // 0.2 is an experimental threshold; can be tuned
break;
}
}
We then discuss its equivalence to the typical SVD-based method and their comparison.
The aforementioned linear least-squares (LLS) method fits the general plane equation Ax + By + Cz + D = 0, whereas the SVD-based method replaces D with D = - (Ax0 + By0 + Cz0) and fits the plane equation A(x-x0) + B(y-y0) + C(z-z0) = 0, where (x0, y0, z0) is the mean of all points that serves as the origin of the new local coordinate frame.
Comparison between two methods:
The LLS fitting method is much faster than the SVD-based method, and is suitable for use when points are known to be roughly in a plane shape.
The SVD-based method is more numerically stable when the plane is far away from origin, because the LLS method would require more digits after decimal to be stored and processed in such cases.
The LLS method can detect outliers by checking the dot product residual between each point and the estimated normal vector, whereas the SVD-based method can detect outliers by checking if the smallest eigenvalue of the covariance matrix is significantly smaller than the two larger eigenvalues (i.e. checking the shape of the covariance matrix).
We finally provide a test case in C++ and MATLAB.
// Test case in C++ (using LLS fitting method)
matA(0,0) = 5.4637; matA(0,1) = 10.3354; matA(0,2) = 2.7203;
matA(1,0) = 5.8038; matA(1,1) = 10.2393; matA(1,2) = 2.7354;
matA(2,0) = 5.8565; matA(2,1) = 10.2520; matA(2,2) = 2.3138;
matA(3,0) = 6.0405; matA(3,1) = 10.1836; matA(3,2) = 2.3218;
matA(4,0) = 5.5537; matA(4,1) = 10.3349; matA(4,2) = 1.8796;
// With this sample data, LLS fitting method can produce the following result
// fitted normal vector = (-0.0231143, -0.0838307, -0.00266429)
// unit normal vector = (-0.265682, -0.963574, -0.0306241)
// D = 11.4943
% Test case in MATLAB (using SVD-based method)
points = [5.4637 10.3354 2.7203;
5.8038 10.2393 2.7354;
5.8565 10.2520 2.3138;
6.0405 10.1836 2.3218;
5.5537 10.3349 1.8796]
covariance = cov(points)
[V, D] = eig(covariance)
normal = V(:, 1) % pick the eigenvector that corresponds to the smallest eigenvalue
% normal = (0.2655, 0.9636, 0.0306)

Resources