I know the distance to various points on a plane, as it is being viewed from an angle. I want to find the equation for this plane from just that information (5 to 15 different points, as many as necessary).
I will later use the equation for the plane to estimate what the distance to the plane should be at different points; in order to prove that it is roughly flat.
Unfortunately, a google search doesn't bring much up. :(
If you, indeed, know distances and not coordinates, then it is ill-posed problem - there is infinite number of planes that will have points with any number of given distances from origin.
This is easy to verify. Let's take shortest distance D0, from set of given distances {D0..DN-1} , and construct a plane with normal vector {D0,0,0} (vector of length D0 along x-axis). For each of remaining lengths we now have infinite number of points that will lie in this plane (forming circles in-plane around (D0,0,0) point). Moreover, we can rotate all vectors by an arbitrary angle and get a new plane.
Here is simple picture in 2D (distances to a line; it's simpler to draw ;) ).
As we can see, there are TWO points on the line for each distance D1..DN-1 > D0 - one is shown for D1 and D2, and the two other for these distances would be placed in 4th quadrant (+x, -y). Moreover, we can rotate our line around origin by an arbitrary angle and still satisfy given distances.
I'm going to skip over the process of finding the best fit plane, it's been handled in some other answers, and talk about something else.
"Prove" takes us into statistical inference. The way this is done is you make a formal hypothesis "the surface is flat" and then see if the data supports rejecting this hypothesis at some confidence level.
So you can wind up saying "I'm not even 1% sure that the surface isn't flat" -- but you can't ever prove that it's flat.
Geometry? Sounds like a job for math.SE! What form will the equation take? Will it be a plane?
I will assume you want an accurate solution.
Find the absolute positions with geometry
Make a best fit regression line in C++ in 2 of the 3 dimensions.
Related
I'm working on an application were I have a set of Contours(each one representing a Potential Line) and I wanna check "How straight" is that contour/shape.
The article I am using as a refrence uses the following technique:
It Matches a "segmented" line crossing the shape like so-
Then grading how "straight" is the line.
Heres an example of the Contours I am working on:
How would you go about implementing this technique?
Is there any other way of checking "How Straight" is a contour\shape?
Regards!
My first guess would be to use a coefficient of determination. That would be, fit a linear line to all your point assuming some reasonable origin where you won't receive rounding errors and calculate R^2.
A more advanced approach, if all contours are disconnected components, would be to calculate the structure model index (the link is for bone morphometry, but they explain the concept and cite the original paper.) This gives you a number that tells you how much your segment is "like a rod". This is just an idea, though. Anything that forms curves or has branches will be less and less like a rod.
I would say that it also depends on what you are using the metric for and if your contours are always generally carrying left to right.
An additional method would be to create the covariance matrix of your points, calculate the eigenvalues from that matrix, and take their ratio (where the ratio is greater than or equal to 1; otherwise, invert the ratio.) This is the basic principle behind a PCA besides the final ratio. If you have a rather linear data set (the data set varies in only one direction) then you will have a very large ratio. As the data set becomes less and less linear (or more uncorrelated) you would see the ratio approach one. A perfectly linear data set would be infinity and a perfect circle one (I believe, but I would appreciate if someone could verify this for me.) Also, working in two dimensions would mean the calculation would be computationally cheap and straight forward.
This would handle outliers very well and would be invariant to the rotation and shape of your contour. You also have a number which is always positive. The only issue would be preventing overflow when dividing the two eigenvalues. Then again you could always divide the smaller eigenvalue by the larger and your metric would be bound between zero and one, one being a circle and zero being a straight line.
Either way, you would need to test if this parameter is sensitive enough for your application.
One example for a simple algorithm is using the dot product between two segments to determine the angle between them. The formula for dot product is:
A * B = ||A|| ||B|| cos(theta)
Solving the equation for cos(theta) yields
cos(theta) = (A * B / (||A|| ||B||))
Since cos(0) = 1, cos(pi) = -1.0 and you're checking for the "straightness" of the lines, a line whose normalization of cos(theta) angles is closest to -1.0 is the straightest.
straightness = SUM(cos(theta))/(number of line segments)
where a straight line is close to -1.0, and a non-straight line approaches 1.0. Keep in mind this is a cursory evaluation of this algorithm and it obviously has edge cases and caveats that would need to be addressed in an implementation.
The trick is to use image moments. In short, you calculate the minimum inertia around an axis, the inertia around an axis perpendicular to this, and the ratio between them (which is always between 0 and 1; since inertia is non-negative)
For a straight line, the inertia along the line is zero, so the ratio is also zero. For a circle, the inertia is the same along all axis so the ratio is one. Your segmented line will be 0.01 or so as it's a fairly good match.
A simpler method is to compare the circumference of the the convex polygon containing the shape with the circumference of the shape itself. For a line, they're trivially equal, and for a not too crooked shape it's still comparable.
I have a set of point cloud, and I would like to test if there is a corner in a 3D room. So I would like to discuss my approach and if there is a better approach or not in terms of speed, because I want to test it on mobile phones.
I will try to use hough tranform to detect lines, then I will try to see if there are three lines that are intersecting and they make a two plane that are intersecting too.
If the point cloud data comes from a depth sensor, then you have a relatively dense sampling of your walls. One thing I found that works well with depth sensors (e.g. Kinect or DepthSense) is a robust version of the RANSAC procedure that #MartinBeckett suggested.
Instead of picking 3 points at random, pick one point at random, and get the neighboring points in the cloud. There are two ways to do that:
The proper way: use a 3D nearest neighbor query data structure, like a KD-tree, to get all the points within some small distance from your query point.
The sloppy but faster way: use the pixel grid neighborhood of your randomly selected pixel. This may include points that are far from it in 3D, because they are on a different plane/object, but that's OK, since this pixel will not get much support from the data.
The next step is to generate a plane equation from that group of 3D points. You can use PCA on their 3D coordinates to get the two most significant eigenvectors, which define the plane surface (the last eigenvector should be the normal).
From there, the RANSAC algorithm proceeds as usual: check how many other points in the data are close to that plane, and find the plane(s) with maximal support. I found it better to find the largest support plane, remove the supporting 3D points, and run the algorithm again to find other 'smaller' planes. This way you may be able to get all the walls in your room.
EDIT:
To clarify the above: the support of a hypothesized plane is the set of all 3D points whose distance from that plane is at most some threshold (e.g. 10 cm, should depend on the depth sensor's measurement error model).
After each run of the RANSAC algorithm, the plane that had the largest support is chosen. All the points supporting that plane may be used to refine the plane equation (this is more robust than just using the neighboring points) by performing PCA/linear regression on the support set.
In order to proceed and find other planes, the support of the previous iteration should be removed from the 3D point set, so that remaining points lie on other planes. This may be repeated as long as there are enough points and best plane fit error is not too large.
In your case (looking for a corner), you need at least 3 perpendicular planes. If you find two planes with large support which are roughly parallel, then they may be the floor and some counter, or two parallel walls. Either the room has no visible corner, or you need to keep looking for a perpendicular plane with smaller support.
Normal approach would be ransac
Pick 3 points at random.
Make a plane.
Check if each other point lies on the plane.
If enough are on the plane - recalculate a best plane from all these points and remove them from the set
If not try another 3 points
Stop when you have enough planes, or too few points left.
Another approach if you know that the planes are near vertical or near horizontal.
pick a small vertical range
Get all the points in this range
Try and fit 2d lines
Repeat for other Z ranges
If you get a parallel set of lines in each Z slice then they are probably have a plane - recalculate the best fit plane for the points.
I would first like to point out
Even though this is an old post, I would like to present a complementary approach, similar to Hough Voting, to find all corner locations, composed of plane intersections, jointly:
Uniformly sample the space. Ensure that there is at least a distance $d$ between the points (e.g. you can even do this is CloudCompare with a 'space' subsampling)
Compute the point cloud normals at these points.
Randomly pick 3 points from this downsampled cloud.
Each oriented point (point+plane) defines a hypothetical plane. Therefore, each 3 point picked define 3 planes. Those planes, if not parallel and not intersecting at a line, always intersect at a single point.
Create a voting space to describe the corner: The intersection of the 3 planes (the point) might a valid parameterization. So our parameter space has 3 free parameters.
For each 3 points cast a vote in the accumulator space to the corner point.
Go to (2) and repeat until all sampled points are exhausted, or enough iterations are done. This way we'll be casting votes for all possible corner locations.
Take the local maxima of the accumulator space. Depending on the votes, we'll be selecting the corners from intersection of the largest planes (as they'll receive more votes) to the intersection of small planes. The largest 4 are probably the corners of the room. If not, one could also consider the other local maxima.
Note that the voting space is a quantized 3D space and the corner location will be a rough estimate of the actual one. If desired, one could store the planes intersection at that very location and refine them (with iterative optimization similar to ICP or etc) to get a very fine corner location.
This approach will be quite fast and probably very accurate, given that you could refine the location. I believe it's the best algorithm presented so far. Of course this assumes that we could compute the normals of the point clouds (we can always do that at sample locations with the help of the eigenvectors of the covariance matrix).
Please also look here, where I have put out a list of plane-fitting related questions at stackoverflow:
3D Plane fitting algorithms
I have points in 3D which make 2 or 3 side of rectangle. How I can calculate the coordinates of the cube's corners? Is it possible?
Updated: https://github.com/CPIGroup/3d-Camera-scanDimensions
This is just an idea, not a proven method.
First, find the planes. Randomly select 3 points, find a plane that passes through them, normalize the 4 parameters. Repeat 1000 or so times. You will end up with 1000 4-tuples of numbers. Use one of the clustering analysis methods to find 2 or 3 groups of 4-tuples that are very close together. Average each of the groups. These will be, approximately, planes of your box's sides.
Now make them more precise. For each plane, find all points that are close to it but not close to other planes (for some value of "close", perhaps to be found using a clustering method too). For each such group of points, find a best fit plane using least squares.
If you have three planes, great; intersect them and you have a vertex and three edges. For two planes, you only have one edge. Either way, you can now try to find other edges. For simplicity, consider your plane to be an XY plane and your known edge an X axis. You now need to find the leftmost (rightmost) vertical line such that most of the points are to the left (resp. right) of it. Project all the points to the X axis. You now have a 1-dimensional case of your original problem: there is a lot of random points on some interval, find the interval. Use a clustering method again.
I'm not super experienced with this, but possibly you could use RANSAC ?
There seem to be many papers on the plane detection from pointclouds using RANSAC
Also you might want to have a look at the Point Clouds Library(PCL).It's a pretty impressive project with many useful features including also planar segmentation
As soon as the planes are detected, it should be a matter of finding the edges/corners which should be a lot simpler.
I am working on an asteroids clone. Everything is 2D, and written in C++.
For the asteroids, I am generating random N-sided polygons. I have guaranteed that they are Convex. I then rotate them, give them a rotspeed, and have them fly through space. It all works, and is very pretty.
For collision, I'm using an Algorithm I thought of myself. This is probably a bad idea, and if push comes to shove, I'll probably scrap the whole thing and find a tutorial on the internet.
I've written and implemented everything, and the collision detection works alright.... most of the time. It will randomly fail when there's obviously a collision on screen, and sometimes indicate collision when nothing is touching. Either I have flubbed my implementation somewhere, or my algorithm is horrible. Due to the size/scope of my implementation (over several source files) I didn't want to bother you with that, and just wanted someone to check that my algorithm is, in fact, sound. At that point I can go on a big bug hunt.
Algorithm:
For each Asteroid, I have a function that outputs where each vertex should be when drawing the asteroid. For each pair of adjacent Vertices, I generate the Formula for the line that they sit on, y=mx+b format. I then start with one of my ships vertices, testing that point to see whether it is inside the asteroid. I start by plugging in the X coordinate of the point, and comparing the output to the Actual Y value. This tells me if the point is above or below the line. I then do the same with the Center of the Asteroid, to determine which half of the line is considered "Inside" the asteroid. I then repeat for each pair of Vertices. IF I ever find a line for which my point is not on the same side as the center of the asteroid, I know there is no collision, and exit detection for that point. Since there are 3 points on my ship, I then have to test for the next point. If all 3 points exit early, then There are no collisions for any of the points on the ship, and we're done. If any point is bound on all sides by the lines made up by the asteroid, then it is inside the asteroid, and the collision flag is set.
The two Issues I've discovered with this algorithm is that:
it doesn't work on concave polygons, and
It has problems with an Edge case where the Slope is Undefined.
I have made sure all polygons are Convex, and have written code to handle the Undefined Slope issue (doubles SHOULD return NAN if we divide by 0, so it's pretty easy to test for that).
So, should this work?
The standard solution to this problem is using the separating axis theorem (SAT). Given two convex polygons, A and B, the algorithm basically goes like this:
for each normal N of the edges of A and B
intervalA = [min, max] of projecting A on N
intervalB = [min, max] of projecting B on N
if intervalA doesn't overlap intervalB
return did not collide
return collided
I did something similar to compute polygon intersections, namely finding if a vertex sits within a given polygon.
Your algorithm is sound, and indeed does not work for concave polys. The line representation you chose is also problematic at slopes approaching infinity. I chose to use a couple of vectors for mine, one for the line direction, and one for a reference point on the line. From these, I can easily derive a parameterized equation of the line, and use that in various ways to find intersections with other shapes.
P = S + t * D
Any point P of the line can be caracterized by its coordinate t on the the line, given the above relation, where S is the reference point, and D the direction vector.
This representation lets you easily define which parts of the plane is the positive and the negative one (ie. above and below the line), thanks to the direction orientation. Now, any region of the plane can be defined as an intersection of several lines' negative or positive subplanes. So your "point within polygon" algorithm could be slightly changed to use that representation, with the added constraint of all the direction pointing clockwise, and testing for the point being in the negative subplane of all the lines (so you don't need the centre of the polygon anymore).
The formula to compute the side of a point wrt a line I used is the following:
(xs - xp) * yd - (ys - yp) * xd
The slope issue appears here when point P is close to S.
That representation can be computed using the edge vertices, but in order to have correct subplanes, you must keep your vertices in your polygon in condecutive orders.
For concave polygons, the problem is a bit more complicated: briefly, you have to test that the point is between two consecutive convex edges. This can be achieved by checking the coordinate of the point on the edge when projected on it, and ensuring it stands between 0 and length(edge) (assuming that the direction is normalized). Note that it boils down to check if the point belongs to a triangle within the polygon.
I have some 3D Points that roughly, but clearly form a segment of a circle. I now have to determine the circle that fits best all the points. I think there has to be some sort of least squares best fit but I cant figure out how to start.
The points are sorted the way they would be situated on the circle. I also have an estimated curvature at each point.
I need the radius and the plane of the circle.
I have to work in c/c++ or use an extern script.
You could use a Principal Component Analysis (PCA) to map your coordinates from three dimensions down to two dimensions.
Compute the PCA and project your data onto the first to principal components. You can then use any 2D algorithm to find the centre of the circle and its radius. Once these have been found/fitted, you can project the centre back into 3D coordinates.
Since your data is noisy, there will still be some data in the third dimension you squeezed out, but bear in mind that the PCA chooses this dimension such as to minimize the amount of data lost, i.e. by maximizing the amount of data that is represented in the first two components, so you should be safe.
A good algorithm for such data fitting is RANSAC (Random sample consensus). You can find a good description in the link so this is just a short outline of the important parts:
In your special case the model would be the 3D circle. To build this up pick three random non-colinear points from your set, compute the hyperplane they are embedded in (cross product), project the random points to the plane and then apply the usual 2D circle fitting. With this you get the circle center, radius and the hyperplane equation. Now it's easy to check the support by each of the remaining points. The support may be expressed as the distance from the circle that consists of two parts: The orthogonal distance from the plane and the distance from the circle boundary inside the plane.
Edit:
The reason because i would prefer RANSAC over ordinary Least-Squares(LS) is its superior stability in the case of heavy outliers. The following image is showing an example comparision of LS vs. RANSAC. While the ideal model line is created by RANSAC the dashed line is created by LS.
The arguably easiest algorithm is called Least-Square Curve Fitting.
You may want to check the math,
or look at similar questions, such as polynomial least squares for image curve fitting
However I'd rather use a library for doing it.