Plane construction from 3D Points in OpenCV - opencv

I would like to construct a plane from a list of 3D points in OpenCV. I would like to obtain the result by finding the four parameters in the following form: Ax+By+Cz+D = 0. Would anyone suggest me a way to do it?

If the data does not contain outliers and does not contain more than one plane. Furthermore, all the points lay exactly on a plane (the data is not noisy), it is so simple:
Pick up three random points which are not lay on the same line.
solve this system of linear equations:
x1+by1+cz1+d = 0
x2+by2+cz2+d = 0
x3+by3+cz3+d = 0
then :
A= Choose any number you want in order to match your scale.
B= b*A
C= c*A
D= d*A
If the data is noisy or contains outliers or more than plane (or both) you need then some kind of Robust Estimation techniques. Search for RANSAC as a start.
if you are familar with RANSAC you can see this example here (it is about lines you can simply generlize it to deal with plane)

Answer by leveraging OpenCV
If you want to have equation solved by 3 points, just like follows:
ax + by + cz = 1
Example
you have three points: cv::Point3f p1, p2 and p3, and here is the code:
cv::Matx33f M(p1.x, p1.y, p1.z,
p2.x, p2.y, p2.z,
p3.x, p3.y, p3.z);
cv::Vec3f d(1, 1, 1);
cv::Vec3f coef = M.inv() * d;
Then, a, b, c are coef(0), coef(1), coef(2) sequentially.

Related

Code for a multiple quadratic (or polynomial) least squares (surface fit)?

for a machine vision project I am trying to search image data for quadratic surfaces (f(x,y) = Ax^2+Bx+Cy^2+Dy+Exy+F). My plan is to iterate through regions of data and perform a surface-fit, look at the error, see if it's a continuous surface (which would probably indicate a feature in the image).
I was previously able to find quadratic curves (f(x) = Ax^2+Bx+C) in the image data by sampling lines, by using the equations on this site
Link
this worked well, was promising, but it would be much more useful for my task to find 2-D regions that form continuous surfaces.
I see lots of articles indicating that least squares regressions scales up to multiple dimensions, but I'm not able to find code for this Hopefully there is a "closed form" (non-iterative, just compute from your data points) solution, like described above for 1D data. Does anybody know of some source or pseudocode that accomplishes this? Thanks.
(Sorry if my terminology is a bit off.)
I'm not sure what your background is, but if you know some linear algebra you will find linear least squares on wikipedia useful.
Lets take the following example. Say we have the following image
and we want to know how well this fits to a 2D quadratic function in a least squares sense.
Probably the most straightforward way to solve the problem is to compute the optimal coefficients in a least squares sense, then check the error.
First we need to describe the matrices.
Let X be a matrix containing every x,y coordinate in the image, taking the form
X = [x1 x1^2 y1 y1^2 x1*y1 1;
x2 x2^2 y2 y2^2 x2*y2 1;
...
xN xN^2 yN yN^2 xN*yN 1];
For the example image above, X would be a 100x6 matrix.
Let y be the image intensity values in a vector of the form
y = [img(x1,y1);
img(x2,y2);
...
img(xN,yN)]
In this case y is a 100 element column vector.
We want to minimize the least squares objective function S with respect to the vector of coefficients b
S(b) = |y - X*b|^2
where |.| is the L2 norm and b is the desired coefficients
b = [A;
B;
C;
D;
E;
F]
Taking the vector derivative of S(b) with respect to b, setting to zero, and solving for b leads to the standard least squares solution.
b = inv(X'X)*X'*y
where inv is the matrix inverse, ' is transpose, and * is matrix multiplication.
MATLAB example.
% Generate an image
% define x,y coordinates for each location in the image
[x,y] = meshgrid(1:10,1:10);
% true coefficients
b_true = [0.1 0.5 0.3 -0.4 0.4 124];
% magnitude of noise
P = 2;
% create image
img = b_true(1).*x + b_true(2).*x.^2 + b_true(3).*y + b_true(4).*y.^2 + b_true(5).*x.*y + b_true(6);
noise = P*randn(10,10);
img = img + noise;
% Begin least squares optimization
% create matrices
X = [x(:) x(:).^2 y(:) y(:).^2 x(:).*y(:) ones(size(x(:)))];
y = img(:);
% estimated coefficients
b = (X.'*X)\(X.')*y
% mean square error (expected to be near P^2)
E = 1/numel(y) * sum((y - X*b).^2)
Output
b =
0.0906
0.5093
0.1245
-0.3733
0.3776
124.5412
E =
3.4699
In your application you would probably want to define some threshold such that when E < threshold you accept the image (or image region) as a quadratic polynomial.

Ortho projection of 3D points with a vector

I have 3D points and I need to make an 2D orthographic projection of them onto a plane that is defined by the origin and a normal n. The meaning of this is basically looking at the points from the top (given the vertical vector). How can I do it?
What I'm thinking is:
project point P onto the 3D plane: P - P dot n * n
look at the 3D plane from the "back" in respect to the normal (not sure how to define this)
do an ortho projection using max-min coordinates of the points in the plane to define the clipping
I am working with iOS.
One way to do this would be to:
rotate the coordinate system so that the plane of interest lies in the x-y plane, and the normal vector n is aligned with the z-axis
project the points onto the x-y plane by setting their z-components to 0
Set up the coordinate transformation
There are infinitely many solutions to this problem since we can always rotate a solution in the x-y plane to get another valid solution.
To fix this, let's choose a vector v lying in the plane that will line up with the x-axis after the transformation. Any vector will do; let's take the vector in the plane with coordinates x=1 and y=0.
Since our plane intersects the origin, its equation is:
x*n1 + y*n2 + z*n3 = 0
z = -(x*n1 + y*n2)/n3
After substituting x=1, y=0, we see that
v = [1 0 -n1/n3]
We also need to make sure v is normalized, so set
v = v/sqrt(v1*v1 + v2*v2 + v3*v3)
EDIT: The above method will fail in cases where n3=0. An alternative method to find v is to take a random point P1 from our point set that is not a scalar multiple of n and calculate v = P1 - P1 dot n * n, which is the projection of P1 into the plane. Just keep searching through your points until you find one that satisfies (P1 dot n/norm(n)) != P1 and this is guaranteed to work.
Now we need a vector u that will line up with the y-axis after the transformation. We get this from the cross product of n and v:
u = n cross v
If n and v are normalized, then u is automatically normalized.
Next, create the matrix
M = [ v1 v2 v3 ]
[ u1 u2 u3 ]
[ n1 n2 n3 ]
Transform the points
Now given a 3 by N array of points P, we just follow the two steps above
P_transformed = M*P
P_plane = set the third row of P_transformed to zero
The x-y coordinates of P_plane are now a 2D coordinate system in the plane.
If you need to get the 3D spatial coordinates back, just do the reverse transformation with P_space = M_transpose*P_plane.

Finding isocurve from triangulation with known uv at all vertices

I have a 3D triangulated mesh which is similar to a curved piece of paper in that it has a 4 edges and lives in 3-dimensional space. Edges may be different lengths and curve differently, but it could theoretically be continually morphed to look like a piece of paper. A uv coordinate has been assigned to every vertex and the range of u and v is between 0 and 1. Some vertices are obviously on the border. For the bottom border u is in the range [0,1] and v is 0. Top border vertices have u within [0,1] and v = 1. The left and right borders have u = 0 or u = 1 (respectively) with v within [0,1].
Now think about the "isocurve" where u = 0.5. This would be the "line" (or collection of line segments?) from bottom to top of the "middle" of the surface. How would I go about finding that?
Or, let's say I wanted to find the 3D coordinate corresponding to the uv coordinate (0.2,0.7). How would I get there?
I don't want to implement this by putting data through a renderer (OpenGL, etc). I'm sure there must be a standard method. It feels like the inverse of a texture mapping function.
Essentially both of your questions boil down to the same thing: how to convert between UV and XYZ coordinates.
This is an interpolation problem. Considering a single triangle in your mesh, you know both the UV and XYZ coordinates at the 3 vertices. As such, you have the right amount of data to interpolate X,Y,Z as linear functions of U,V:
X(U,V) = a0 + a1*U + a2*V
Y(U,V) = b0 + b1*U + b2*V
Z(U,V) = c0 + c1*U + c2*V
The problem then becomes how to determine the coefficients ai,bi,ci. This can be done by solving a set of linear equations based on the given vertex data. For example, the X coefficients for a given triangle can be found by solving:
[X1] [1 U1 V1] [a0]
[X2] = [1 U2 V2] * [a1]
[X3] [1 U3 V3] [a2]
Once you have all of these coefficients for each triangle in the mesh you can then determine an XYZ coordinate for any UV pair:
1. Locate the triangle that contains the UV point.
2. Evaluate the X(U,V),Y(U,V),Z(U,V) functions for the given triangle.

How to find straight line segments nearest to a straight line segment?

consider an image matrix in which i have multiple line segments. And i have information's like start point, end points, length of the line segment, centroid and slope of all those line segments. In this scenario how do i find line segments that are nearest to a particular line segment. Also once i got nearest line segments is it possible to detect rectangles if they exist? .An example image is in this link sample.
The geometry of segment/segment distance is not so simple.
Imagine two line segments in general position, and dilate one of them. I mean, draw two parallel segments at a distance d and two half circles of radius d centered on the endpoints. This is the locus of constant distance d from the segment. You need to find the smallest d such that the other segment is hit.
You can decompose the plane in three areas: the band between the two perpendiculars through the endpoints, and the two outer half planes. You can clip the other segment in these three areas and process the clipped segments separately.
Inside the band, either the segments intersect and the distance is zero. Or they don't and the distance is the shortest distance between the line of support of the first segment and the endpoints of the other.
Inside the half planes, the distance is the shortest of the distances between the considered endpoint and both endpoints of the other segment, and the distance between the endpoint and the other segment, provided then endpoint projects inside the other segment.
ALTERNATIVE:
Maybe it is easier to use the parametric equations of the two segments and minimize the (squared) distance, like:
Min(p, q) ((Xa (1-p) + Xb p) - (Xc (1-q) + Xd q))^2 + ((Ya (1-p) + Yb p) - (Yc (1-q) + Yd q))^2 under constraints 0 <= p, q <=1.
First, you have to encode all the points in homogeneous coordinates [x, y, 1]T since this creates a symmetric relations between lines and points. Namely, in homogeneous coordinates the intersection of two lines l1 and l2 is a point p=l1xl2, where x means vector product. By the same, coin a line that passes through two points p1, p2 is l=p1xp2. Line that lie on a segment can be expressed as l=p1xp2=[a, b, c]T. The line equation then will be lT.p=0 or in Cartesian coordinates a*x+b*y+c=0
As for your task, there are two cases:
1. segments cross and then their intersection can be simply calculated as l1xl2;
2. segments don’t cross and then the closest points between two lines is the closest point between two of their 4 endpoints. To calculate 4 possible distances and choose the smallest distance between a line segment x1 and x2 and a point x0 use this formula: (x2-x1)x(x1-x0)/|x2-x1|
Let the segments be AB and CD, and running parameters along them p and q, such that 0 <= p, q <= 1.
Using vectors, the squared distance between any two points on the segments is given by:
D² = (AC - p AB + q CD)²
Let us minimize this expression by zeroing the derivatives wrt p and q:
AB.(AC - p AB + q CD) = 0
CD.(AC - p AB + q CD) = 0
When AB and CD are not parallel, this implies AC - p AB + q CD = 0, which gives you the intersection point by solving a 2x2 system, and the distance is zero.
But it can turn out that p (or q) falls out of the allowed range, let p < 0 (or p > 1). In this case, we recast the problem with p = 0 (p = 1). This amounts to finding the distance (A, CD).
D² = (AC + q CD)²
yielding
CD.(AC + q CD) = 0
even easier.
And if it turns out that q is also out of range, let q < 0, we end up with the distance (A, C):
D² = AC²
Similarly for the other out-of-range cases.
In case of parallel segments, the 2x2 system is indeterminate (both equations are equivalent). You need to solve:
CD.AC - p CD.AB + q CD² = 0
It suffices to try all four combinations with p/q = 0/1 and see if the left hand side takes different signs. This proves that there exists a solution and the distance is the same as distance (A, CD). Otherwise, the answer is one of the endpoint-to-endpoint distance.

How to determine curvature of a cubic bezier path at an end point

I've been working on a project where I use Bezier paths to draw the curves I need. Each basic shape in my project consists of three cubic Bezier curves arranged end to end so that the slopes match where they meet.
The basic problem I need to solve is whether the compound curve made of the three Bezier curves intersects with itself. After thinking about it for a while, I've figured out that given the constraints of the curves, I can simplify the task to something else:
The curvature of each of the three Bezier paths should be opposite in curvature direction relative to the curve it's abutted to. In other words, there should be an inflection point where one bezier curve abuts to another one. If that is not the case, I want to reject the parameter set that generated the curves and select a different set.
In any case, my basic question is how to detect whether there is an inflection point where the curves abut each other.
In the illustration, each of the three Bezier curves is shown using a different color. The left black curve curves in the opposite direction from the red curve at the point where they meet, but the right black curve curves in the same direction. There is an inflection point where the red and left black curve meet but not where the red and right black curve meet.
Edit:
Below, I've added another image, showing the polygon enclosing the Bezier path. The crossing lines of the polygon, shown in the black curve, tests for an inflection point, not a loop. I'm guessing that one curve intersecting another can be tested by checking whether the enclosing polygons intersect, as illustrated by the red and blue curves.
P.S. Since there has been some question about the constraints, I will list some of them here:
The left most point and the rightmost point have the same y value.
The x value of the control point of the leftmost point is less than
the x value of the control point of the rightmost point. This keeps
the black and blue curves from intersecting each other.
The slope at the leftmost and rightmost points is within about +/- 10
degrees of horizontal.
The intersection of the black and red curves, and the intersection of
the red and blue curves divide the full curve in approximately
thirds. I don't have exact numbers, but a sample bound would be that
the x value of the left end of the red curve is somewhere between 25%
and 40% of the x value of the rightmost point.
The y value of the points of intersection are +/- some small fraction
of the overall width.
The slope at the intersections is > 0.6 and < 3.0 (positive or
negative).
The equation for curvature is moderately simple. You only need the sign of the curvature, so you can skip a little math. You are basically interested in the sign of the cross product of the first and second derivatives.
This simplification only works because the curves join smoothly. Without equal tangents a more complex test would be needed.
The sign of curvature of curve P:
ax = P[1].x - P[0].x; // a = P1 - P0
ay = P[1].y - P[0].y;
bx = P[2].x - P[1].x - ax; // b = P2 - P1 - a
by = P[2].y - P[1].y - ay;
cx = P[3].x - P[2].x - bx*2 - ax; // c = P3 - P2 - 2b - a
cy = P[3].y - P[2].y - by*2 - ay;
bc = bx*cy - cx*by;
ac = ax*cy - cx*ay;
ab = ax*by - bx*ay;
r = ab + ac*t + bc*t*t;
Note that r is the cross product of the first and second derivative and the sign indicate the direction of curvature. Calculate r at t=1 on the left curve and at t=0 on the right curve. If the product is negative, then there is an inflection point.
If you have curves U, V and W where U is the left black, V is the middle red, and W is the right black, then calculate bc, ac, and ab above for each. The following test will be true if both joins are inflection points:
(Uab+Uac+Ubc)*(Vab) < 0 && (Vab+Vac+Vbc)*(Wab) < 0
The equation for curvature has a denominator that I have ignored. It does not affect the sign of curvature and would only be zero if the curve was a line.
Math summary:
// start with the classic bezier curve equation
P = (1-t)^3*P0 + 3*(1-t)^2*t*P1 + 3*(1-t)*t^2*P2 + t^3*P3
// convert to polynomial
P = P0 + 3*t*(P1 - P0) + 3*t^2*(P2 - 2*P1 + P0) + t^3*(P3 - 3*P2 + 3*P1 - P0)
// rename the terms to a,b,c
P = P0 + 3at + 3btt + cttt
// find the first and second derivatives
P' = 3a + 6bt + 3ctt
P" = 6b + 6ct
// and the cross product after some reduction
P' x P" = ab + act + bctt
One of the deterministic ways to check if a bezier curve has a double-point or self-intersection is to calculate the inverse equation and evaluate the root, since the inversion equation of a bezier curve is always zero at the point of self-intersection.
As detailed in this example by T.W.Sederberg –course notes. Then for detecting whether two bezier curves (red and black in the question) intersect with each other, there are a few methods, binary subdivision (easier to implement) and bezier clipping (very good balance of efficiency and code complexity), implicitization (not worth it).
But a more efficient way to do it probably is to subdivide the curves into small line-segments and find the intersection. Here is one way to do it. Especially when you are interested in knowing whether the path self-intersect, but not in an accurate point of intersection.
If you have well defined assumptions about the locations of the CVs of the piece-wise bezier curves (or poly-bezier as #Pomax mentioned above), you can try out the curvature based method as you have mentioned in your question.
Here is a plot of the curvature on a similar path as in the question. Under similar circumstances, it seems this could give you the solution you are looking for. Also, in case of a cusp, it seems this quick and dirty method still works!
# Victor Engel : Thank you for the clarification. Now the solution is even easier.
In short, look at torques of both curves. If they pull together, the "inflexion" occurs, if they are opposed, the "curvature" continues.
Remark: Words "inflexion" and "curvature" have here a more intuitive nature, than a strict mathematical meaning, therefore apostrophes are used.
As before, a set of points P0,P1,P2,P3 defines the first cubic Bezier curve, and Q0,Q1,Q2 and Q3 the second.
Moreover, 4 vectors: a,b,u,w will be useful.
a = P2P3
b = P1P2
u = Q0Q1
v = Q1Q2
I will omit a C1-continuity checking, it's already done by you. ( This means P3=Q0 and a x u=0)
Finally, Tp and Tq torques will appear.
Tp = b x a
( "x" means a vector product, but Tp on the planar is treated like a simple number, not a vector. }
if Tp=0 {very rarely vectors a, b can be parallel.}
b = P0P1
Tp = b x a
if Tp=0
No! It's can't be! Who straightened the curve???
STOP
endif
endif
Tq = u x v
if Tq=0 {also vectors u, v can be parallel.}
v = Q2Q3
Tq = u x v
if Tq=0
Oh no! What happened to my curve???
STOP
endif
endif
Now the final test:
if Tp*Tq < 0 then Houston! We have AN "INFLEXION"!
else WE CONTINUE THE TURN!
Suppose you have 4 points: P0, P1, P2 and P3, which describes a cubic Bezier's curve (cBc for short) . P0 and P3 are starting and ending points, P1 and P2 are directional points.
When P0, P1 and P2 are collinear, then cBc has an inflection point at P0.
When P1, P2 and P3 are collinear, then cBC has an inflection point at P3.
Remarks.
1) Use the vector product ( P0P1 x P1P2 and P1P2 x P2P3 respectively) to check the collinearity. The result should be a zero-lenght vector.
2) Avoid the situation where P0, P1, P2 and P3 are all collinear, the cBc they create aren't curves in the common sense.
Corollary.
When P1=P2, cBc has inflection points at both sides.

Resources