Align point clouds via 3 points correlation? - math
Let's say I have 3 point clouds: first that has 3 points {x1,y1,z1}, {x2,y2,z2}, {x3,y3,z3} and second point cloud that has same points as {xx1, yy1, zz1}, {xx2,yy2,zz2}, {xx3,yy3,zz3}... I assume to align second point cloud to first I have to multiply second one's points by T[3x3matrix].
1) So how do I find this transform matrix(T) ? I tried to do the equations by hand, but failed to solve them. Is there an solution somewhere, cause I'm pretty sure I'm not the first one to stumble into the problem.
2) I assume that matrix might include skewing and shearing. Is there a way to find matrix with only 7 degrees of freedom (3translation, 3rotation, 1scale)?
The transformation matrix T1 that takes the unit vectors {1, 0, 0}, {0, 1, 0}, and {0, 0, 1} to {x1, y1, z1}, {x2, y2, z2}, {x3, y3, z3} is simply
| x1 x2 x3 |
T1 = | y1 y2 y3 |
| z1 z2 z3 |
And likewise the transformation T2 that takes those 3 unit vectors to the second set of points is
| xx1 xx2 xx3 |
T2 = | yy1 yy1 yy3 |
| zz1 zz2 zz3 |
Therefore, the matrix that takes the first three points to the second three points is given by T2 * T1-1. If T1 is non-singular, then this transformation is uniquely determined, so it has no degrees of freedom. If T1 is a singular matrix, then there could be no solutions, or there could be infinitely many solutions.
When you say you want 7 degrees of freedom, this is somewhat of a misuse of terminology. In the general case, this matrix is composed of 3 rotational degrees of freedom, 3 scaling degrees, and 3 shearing degrees, making a total of 9. You can figure out these parameters by performing a QR factorization. The Q matrix gives you the rotational parameters, and the R matrix gives you the scaling parameters (along the diagonal) and the shearing parameters (above the diagonal).
Approach of Adam Rosenfield is correct. But solution as T2 * Inv (T1) is wrong. Since in Matrix multiplication A * B != B * A : Hence result is Inv(T1) * T2
The seven parameter transformation that you are talking about is referred to as a 3d conformal transformation, or sometimes a 3d similarity transformation given that the two clouds are similar. If the two shapes are identical, Adam Rosenfields solution is good. Where there are small differences, and you wish to get a best fit, the most commonly used solution is a Helmert transformation which uses a least squares approach to minimise the residuals. The wikipedia and google stuff on this doesn't seem great at a glance. My reference on this is Ghilani & Wolf's adjustment computations, p345. This is also a great book on matrix math as applied to spatial problems and a good addition to the library.
edit: Adam's 9 parameter version of this transformation is referred to as an affine transformation
Here is an example of computing least-squares estimates of the parameters of a 2D affine transformation in R.
Related
Find biases and scaling factors from multiple measurements to calibrate measurement data
Let's define that we have a process over time, like acceleration, position etc. f(t). This is the ground truth that is happening. Then we apply a measurement process in form m(t) = k*f(t)+b, where k is a scaling factor, and b is bias and noise combined. I assume that the noise is uniform, and with large enough data set will average out, so I don't care much about that. For example I'm measuring rotation with n accelerometers at different lengths from the center of rotation. Then k would be the length from the center of rotation (plus minus all sorts of errors), and b would be bias terms, like gravity etc. Bur for now let's concentrate on the 1D case for simplicity. If we have n measurements, each would have their own scaling factors and biases, is there a way of getting them (relative or absolute) extracted from the data? Thus far I've gotten relative scaling factors out while testing, by numerically differentiating the data, and then comparing the differentiated functions to first one, and then averaging the result. I assume this works because differentiation gets rid of constants, but how would I approach getting out the bias terms? I haven't gotten any good enough solutions, but I've tried to compute the following for the biases: For n=2 measurements: m1(t) = k1 * f(t) + b1 m2(t) = k2 * f(t) + b2 m2(t) - m1(t) = f(t) * (k2 - k1) + b2 - b1 If we make the assumption that b1 = 0, then we get relative (I think) bias between the two functions, and solving for b2 yields: b2 = m2(t) - m1(t) - f(t) * (k2 - k1) And if we are going with relative bias, we can assume that m1(t) == f(t) and getting us: b2 = m2(t) - m1(t)*(k2 - k1 + 1) This is assuming I didn't do any mistakes in calculation. If this is the case I have problem in implementation, but that is more trivial to solve than wrong thinking process.
How can I optimize a formula on data?
I have an analytically derrived formula that looks something like this; y = ((b/x)^n )* e^(-c(x-b)) * D In the formula x is my independent variable. Alle the other lettres on the right side of the equation are parameters that I want to estimate. I have a dataset with y and x. Is there a way by which I can get the most optimal values of the parameters b,c,d and n that minimizes square error of y. I also have alot of constraints, such as b being inbetween 0 and 10, and c being inbetween 0 and 0.5 etc. Is there way I can perform such an optimization problem in R preferrably? And furthermore, is it possible to have parameters b,c and d predicted by other variables in my dataset. This then becomes more of a ML problem, but I want the model to be written in the same way as the first mentioned formula. Thanks in advance
Calculate Normals from Heightmap
I am trying to convert an heightmap into a matrix of normals using central differencing which will later correspond to the steepness of a giving point. I found several links with correct results but without explaining the math behind. T L O R B From this link I realised I can just do: Vec3 normal = Vec3(2*(R-L), 2*(B-T), -4).Normalize(); The thing is that I don't know where the 2* and -4 comes from. In this explanation of central differencing I see that we should divide that value by 2, but I still don't know how to connect all of this. What I really want to know is the linear algebra definition behind this. I have an heightmap, I want to measure the central differences and I want to obtain the normal vector to use later to measure the steepness. PS: the Z-axis is the height.
From vector calculus, the normal of a surface is given by the gradient operator: A height map h(x, y) is a special form of the function f: For a discretized height map, assuming that the grid size is 1, the first-order approximations to the two derivative terms above are given by: Since the x step from L to R is 2, and same for y. The above is exactly the formula you had, divided through by 4. When this vector is normalized, the factor of 4 is canceled. (No linear algebra was harmed in the writing of this answer)
Split a cubic Bézier curve at a point
This question and this question both show how to split a cubic Bézier curve at a particular parameterized value 0 ≤ t ≤ 1 along the curve, composing the original curve shape from two new segments. I need to split my Bézier curve at a point along the curve whose coordinate I know, but not the parameterized value t for the point. For example, consider Adobe Illustrator, where the user can click on a curve to add a point into the path, without affecting the shape of the path. Assuming I find the point on the curve closest to where the user clicks, how do I calculate the control points from this? Is there a formula to split a Bézier curve given a point on the curve? Alternatively (and less desirably), given a point on the curve, is there a way to determine the parameterized value t corresponding to that point (other than using De Casteljau's algorithm in a binary search)? My Bézier curve happens to only be in 2D, but a great answer would include the vector math needed to apply in arbitrary dimensions.
It is possible, and perhaps simpler, to determine the parametric value of a point on the curve without using De Casteljau's algorithm, but you will have to use heuristics to find a good starting value and similarly approximate the result. One possible, and fairly simple way is to use Newton's method such that: tn+1 = tn - ( bx(tn) - cx ) / bx'(tn) Where bx(t) refers to the x component of some Bezier curve in polynomial form with the control points x0, x1, x2 and x3, bx'(t) is the first derivative and cx is a point on the curve such that: cx = bx(t) | 0 < t < 1 the coefficients of bx(t) are: A = -x0 + 3x1 - 3x2 + x3 B = 3x0 - 6x1 + 3x2 C = -3x0 + 3x1 D = x0 and: bx(t) = At3 + Bt2 + Ct + D, bx'(t) = 3At2 + 2Bt + C Now finding a good starting value to plug into Newton's method is the tricky part. For most curves which do not contain loops or cusps, you can simply use the formula: tn = ( cx - x0 ) / ( x3 - x0 ) | x0 < x1 < x2 < x3 Now you already have: bx(tn) ≈ cx So applying one or more iterations of Newton's method will give a better approximation of t for cx. Note that the Newton Raphson algorithm has quadratic convergence. In most cases a good starting value will result in negligible improvement after two iterations, i.e. less than half a pixel. Finally it's worth noting that cubic Bezier curves have exact solutions for finding extrema via finding roots of the first derivative. So curves which are problematic can simply be subdivided at their extrema to remove loops or cusps, then better results can be obtained by analyzing the resulting section in question. Subdividing cubics in this way will satisfy the above constraint.
Simple Orthographic Structure from Motion using R -- Determining Metric Constraints
I would like to build a simple structure from motion program according to Tomasi and Kanade [1992]. The article can be found below: https://people.eecs.berkeley.edu/~yang/courses/cs294-6/papers/TomasiC_Shape%20and%20motion%20from%20image%20streams%20under%20orthography.pdf This method seems elegant and simple, however, I am having trouble calculating the metric constraints outlined in equation 16 of the above reference. I am using R and have outlined my work thus far below: Given a set of images I want to track the corners of the three cabinet doors and the one picture (black points on images). First we read in the points as a matrix w where Ultimately, we want to factorize w into a rotation matrix R and shape matrix S that describe the 3 dimensional points. I will spare as many details as I can but a complete description of the maths can be gleaned from the Tomasi and Kanade [1992] paper. I supply w below: w.vector=c(0.2076,0.1369,0.1918,0.1862,0.1741,0.1434,0.176,0.1723,0.2047,0.233,0.3593,0.3668,0.3744,0.3593,0.3876,0.3574,0.3639,0.3062,0.3295,0.3267,0.3128,0.2811,0.2979,0.2876,0.2782,0.2876,0.3838,0.3819,0.3819,0.3649,0.3913,0.3555,0.3593,0.2997,0.3202,0.3137,0.31,0.2718,0.2895,0.2867,0.825,0.7703,0.742,0.7251,0.7232,0.7138,0.7345,0.6911,0.1937,0.1248,0.1723,0.1741,0.1657,0.1313,0.162,0.1657,0.8834,0.8118,0.7552,0.727,0.7364,0.7232,0.7288,0.6892,0.4309,0.3798,0.4021,0.3965,0.3844,0.3546,0.3695,0.3583,0.314,0.3065,0.3989,0.3876,0.3857,0.3781,0.3989,0.3593,0.5184,0.4849,0.5147,0.5193,0.5109,0.4812,0.4979,0.4849,0.3536,0.3517,0.4121,0.3951,0.3951,0.3781,0.397,0.348,0.5175,0.484,0.5091,0.5147,0.5128,0.4784,0.4905,0.4821,0.7722,0.7326,0.7326,0.7232,0.7232,0.7119,0.7402,0.7006,0.4281,0.3779,0.3918,0.3863,0.3825,0.3472,0.3611,0.3537,0.8043,0.7628,0.7458,0.7288,0.727,0.7213,0.7364,0.6949,0.5789,0.5491,0.5761,0.5817,0.5733,0.5444,0.5537,0.5379,0.3649,0.3536,0.4177,0.3951,0.3857,0.3819,0.397,0.3461,0.697,0.671,0.6821,0.6821,0.6719,0.6412,0.6468,0.6235,0.3744,0.3649,0.4159,0.3819,0.3781,0.3612,0.3763,0.314,0.7008,0.6691,0.6794,0.6812,0.6747,0.6393,0.6412,0.6235,0.7571,0.7345,0.7439,0.7496,0.7402,0.742,0.7647,0.7213,0.5817,0.5463,0.5696,0.5779,0.5761,0.5398,0.551,0.5398,0.7665,0.7326,0.7439,0.7345,0.7288,0.727,0.7515,0.7062,0.8301,0.818,0.8571,0.8878,0.8766,0.8561,0.858,0.8394,0.4121,0.3876,0.4347,0.397,0.38,0.3631,0.3668,0.2971,0.912,0.8962,0.9185,0.939,0.9259,0.898,0.8887,0.8571,0.3989,0.3781,0.4215,0.3725,0.3612,0.3461,0.3423,0.2782,0.9092,0.8952,0.9176,0.9399,0.925,0.8971,0.8887,0.8571,0.4743,0.4536,0.4894,0.4517,0.446,0.4328,0.4385,0.3706,0.8273,0.8171,0.8571,0.8878,0.8766,0.8543,0.8561,0.8394,0.4743,0.4554,0.4969,0.4668,0.4536,0.4404,0.4536,0.3857) w=matrix(w.vector,ncol=16,nrow=16,byrow=FALSE) Then create registered measurement matrix wm according to equation 2 as by wm = w - rowMeans(w) We can decompose wm into a '2FxP' matrix o1 a diagonal 'PxP' matrix e and 'PxP' matrix o2 by using a singular value decomposition. svdwm <- svd(wm) o1 <- svdwm$u e <- diag(svdwm$d) o2 <- t(svdwm$v) ## dont forget the transpose! However, because of noise, we only pay attention to the first 3 columns of o1, first 3 values of e and the first 3 rows of o2 by: o1p <- svdwm$u[,1:3] ep <- diag(svdwm$d[1:3]) o2p <- t(svdwm$v)[1:3,] ## dont forget the transpose! Now we can solve for our rhat and shat in equation (14) by rhat <- o1p%*%ep^(1/2) shat <- ep^(1/2) %*% o2p However, these results are not unique and we still need to solve for R and S by equation (15) by using the metric constraints of equation (16) Now I need to find Q. I believe there are two potential methods but am unclear how to employ either. Method 1 involves solving for B where B=Q%*%solve(Q) then using Cholesky decomposition to find Q. Method 1 appears to be the common choice in literature, however, little detail is given as to how to actually solve the linear system. It is apparent that B is a '3x3' symmetric matrix of 6 unknowns. However, given the metric constraints (equations 16), I don't know how to solve for 6 unknowns given 3 equations. Am I forgetting a property of symmetric matrices? Method II involves using non-linear methods to estimate Q and is less commonly used in structure from motion literature. Can anyone offer some advice as to how to go about solving this problem? Thanks in advance and let me know if I need to be more clear in my question.
can be written as . can be written as . can be written as . so our equations are: So the first equation can be written as: which is equivalent to To keep it short we define now: (I know the spacings are terrably small, but yes, this is a Vector...) So for all equations in all different Frames f, we can write one big equation: (sorry for the ugly formulas...) Now you just need to solve the -Matrix using Cholesky decomposition or whatever...