I am currently working on a project that involves measuring distances all around a robot with a laser module, the robot then has to move based on the points that he gets.
I currently have access to 360 points that represent the distance from the center for each of the corresponding angles. (a distance for 0°, a distance for 1°, etc)
Here's an example of what the points look like when displayed on a 2D surface:
Circular representation of the points
What I'd like to be able to do is, rather than feeding the robot all 360 points, to feed it segments containing multiple points. For instance, the bottom part of the image would be a single segment even though the points are not completely aligned.
My question to you is, is there an existing algorithm that would help me achieve what I am trying to do?
(I'm working in python but that shouldn't really be a factor)
Thanks a lot.
Assuming your points are ordered:
For each point, look ahead by two points, if the middle point is less than some distance away from the segment between the two points, then push your endpoint 1 pt further, and check that now both of the middle points are still within some distance of your line segment. Proceed to do this until false, at which point roll back one pt and generate a segment, then set the end of that segment as the start of your next segment. Also, you could consider angles instead of just distances as there are some cases where that would be favorable. Also, if no segment can be made from a certain start point for several attempts, push the start point forward one (as not everything is going to simplify into segments)
Alternately, you could convert to Cartesian points and use the hough voting algorithm to detect lines from the resulting point-cloud.
Related
I am currently working on a hole detection problem in 3D point cloud data. I am referring to this paper "Detecting Holes in Point Set Surfaces" by Gerhard H Bendels, Ruwen Schnabel and Reinhard Klein. One of the criterions mentioned is an Angle Criterion in which we need to determine angles between consecutive points in a KD Tree(Radially Nearest Neighbors to a given point).
See Image:
Angle between points
I am using Open3D to extract a KD Tree but I believe it is giving me an unsorted list of points rather than a list of consecutive points.
See Image:
List of Nearest neighbors
The point below '______' is the point of interest and rest are it's neighbors. Now my question is,
How do I know which point is next to which point?
And if that's not possible to know, How can I find the angles as shown in the first image.
I just need the angles to find the boundary probability for each point, so an answer would really help me progress.
Thanks
What I've Tried so far..
I have tried generating vectors out of all the points and calculated the angles using dot product. But that seems wrong because I believe I may be calculating dot products between first and third point.
I am dealing with a reverse-engineering problem regarding road geometry and estimation of design conditions.
Suppose you have a set of points obtained from the measurement of positions of a road. This road has straight sections as well as curve sections. Straight sections are, of course, represented by lines, and curves are represented by circles of unknown center and radius. There are, as well, transition sections, which may be clothoids / Euler spirals or any other usual track transition curve. A representation of the track may look like this:
We know in advance that the road / track was designed taking this transition + circle + transition principle into account for every curve, yet we only have the measurement points, and the goal is to find the parameters describing every curve on the track, this is, the transition parameters as well as the circle's center and radius.
I have written some code using a nonlinear optimization algorithm, where a user can select start and end points and fit a circle that to the arc section between them, as it shows in next figure:
However, I don't find a suitable way to take the transition into account. After giving it some thought I came to think that this s because, given a set of discrete points -with their measurement error- representing a full curve, it is not entirely clear where to consider it "begins" and where it "ends" and, moreover, it is less clear where to consider the transition, the proper circle and the exit transition "begin" and "end".
Is there any work on this subject which I may have missed? is there a proper way to fit the whole transition + curve + transition structure into the set of points?
As far as I know, there's no method to fit a sequence clothoid1-circle-clothoid2 into a given set of points.
Basic facts are that two points define a straight, and three points define a unique circle.
The clothoid is far more complex, because you need: The parameter A, the final radius Rf, an initial point px,py, the radius Ri at that point, and the tangent T (angle with X-axis) at that point.
These are 5 data you may use to find the solution.
Due to clothoid coords are calculated by expanded Fresnel integrals (see https://math.stackexchange.com/a/3359006/688039 a little explanation), and then apply a translation & rotation, there's no an easy way to fit this spiral into a set of given points.
When I've had to deal with your issue, what I've done is:
Calculate the radius for triplets of consecutive points: p1p2p3, p2p3p4, p3p4p5, etc
Observe the sequence of radius. Similar values mean a circle, increasing/decreasing values mean a clothoid; Big values would mean a straight.
For each basic element (line, circle) find the most probably characteristics (angles, vertices, radius) by hand or by some regression method. Many times the common sense is the best.
For a spiral you may start with aproximated values, taken from the adjacent elements. These values may very well be the initial angle and point, and the initial and final radius. Then you need to iterate, playing with Fresnel and 'space change' until you find a "good" parameter A. Then repeat with small differences in the other values, those you took from adjacents.
Make the changes you consider as good. For example, many values (A, radius) use to be integers, without decimals, just because it was easier for the designer to type.
If you can make a small applet to do these steps then it's enough. Using a typical roads software helps, but doesn't avoid you the iteration process.
If the points are dense compared to the effective radii of curvature, estimate the local curvature by least square fitting of a circle on a small number of points, taking into account that the curvature is most of the time zero.
You will obtain a plot with constant values and ramps that connect them. You can use an estimate of the slope at the inflection points to figure out the transition points.
Currently I am drawing a 3D curve consisting of 1200...1500 straight micro-lines directed by an array of 3D points (x,y,z), but rendering is a bit slow regardless of used technology (Adobe Flash, Three.js).
The curve is a kind of 3D arc with a 180 degree loop at the end, so I thought that skipping some points in places where the curve is more smooth and predictable will speed up rendering.
Could you suggest some algorithm which can determine how close to a straight line the specific piece of 3D curve is?
Update
I tried to make Three.js to render these points as a single curve and it works really fast. But the different pieces of this curve should be differently colored, so I have to draw it as a bunch of separate lines and the only thing I can do to speed it up is to skip every second point in a region where line is close to a straight line.
I can not use OpenGL (WebGL) because not all browsers support it.
The difference between three points and a straight line can be quantified from the distance of the middle one from the line on which the other two rest. Probably getting the two lengths along the line from either end point to the middle one, dividing the distance by both and summing the two results is the easiest way to turn it into a single number.
So:
as the middle point gets closer to the line, the number goes down;
as the line segment grows longer, variations by the mid point need to be proportionally more extreme; and
greater local slope (as if the middle point were very close to either end) produces a greater error.
You can get the distance from a position to a line by obtaining the vector from any point on the line to the position, using the dot product to work out how much of that is movement along the line and then subtracting that from the total. If you don't normalise your vector along the line first you end up having multiplied by the square of it, so no need for a square root operation on that account. Then for the implied length calculation you can just keep and compare all of those as squared.
I have given the coordinates of 1000 triangles on a plane (triangle number (T0001-T1000) and its coordinates (x1,y1) (x2,y2),(x3,y3)). Now, for a given point P(x,y), I need to find a triangle which contains the point P.
One option might be to check all the triangles and find the triangle that contain P. But, I am looking for efficient solution for this problem.
You are going to have to check every triangle at some point during the execution of your program. That's obvious right? If you want to maximize the efficiency of this calculation then you are going to create some kind of cache data structure. The details of the data structure depend on your application. For example: How often do the triangles change? How often do you need to calculate where a point is?
One way to make the cache would be this: Divide your plane in to a finite grid of boxes. For each box in the grid, store a list of the triangles that might intersect with the box.
Then when you need to find out which triangles your point is inside of, you would first figure out which box it is in (this would be O(1) time because you just look at the coordinates) and then look at the triangles in the triangle list for that box.
Several different ways you could search through your triangles. I would start by eliminating impossibilities.
Find a lowest left corner for each triangle and eliminate any that lie above and/or to the right of your point. continue search with the other triangles and you should eliminate the vast majority of the original triangles.
Take what you have left and use the polar coordinate system to gather the rest of the needed information based on angles between the corners and the point (java does have some tools for this, I do not know about other languages).
Some things to look at would be convex hull (different but somewhat helpful), Bernoullies triangles, and some methods for sorting would probably be helpful.
I am working with a Microchip dsPIC33FJ128GP802. It's a small DSP-based microcontroller, and it doesn't have much power (40 million instructions per second). I'm looking for a way to render a convex (i.e. simple) polygon. I am only dealing with 2D shapes, integer math, and set or clear pixels (i.e. 1 bit per pixel.) I already have routines for drawing fast horizontal and vertical lines (writing up to 16 pixels in 88 cycles), so I would like to use a scanline algorithm.
However, all the algorithms I have found seem to depend on division (which takes 18 cycles on this processor) and floating point math (which is emulated in software and so is very slow; it also takes up a lot of ROM), or assume that I have a large amount of memory. I only have 2K left, ~14K is used for graphics RAM of my 16K. So does anyone know of any good, embedded machine algorithms they can point me to with a simple C or pseudocode implementation which I can implement in assembly? Preferably on the 'net, I don't live near any good bookstores with many programming books.
Thanks. :)
EDIT: Clarification, this is a polygon filling algorithm I'm looking for. I can implement a polygon outline algorithm using Bresenham's line drawing algorithm (as Marc B suggests.)
EDIT #2: I wanted to let everyone know I got a basic algorithm up in Python. Here's a link to the code. Public domain code.
http://dl.dropbox.com/u/1134084/bresenham_demos.py
How about Bresenham's Line algorithm? After some setup, it's pure integer math, and can be adapted to draw a polygon by simple iteration of starting points along the polygon edges.
comments followup:
I'll try to draw this in ASCII, but it'll probably look like crud. Bresenham's can be used to draw a filled polygon by picking a starting edge, and iteratively moving a bresenham line across the canvas parallel to that point.
Let's say you've got some points like this:
*(1)
*(3)
*(2)
*(4)
These are numbered in left-right sort priority, so you pick the left-most starting point (1) and decide if you want to go vertically (start 1,2) or horizontally (1,3). That'd probably depend on how your DSP does its display, but let's go with vertical.
So... You use the 1-2 line as your starting bresenham line. You calculate the starting points of your fill lines by using lines 1-3 and 2-4 as your start/end points. Start a bresenham calculation for each, and draw another Bresenham between those two points. Kinda like:
1.1 -> 2.1, then 1.2 -> 2.2, then 1.3 -> 2.3
etc... until you reach the end of either of those lines. In this case, that'd be when the lower starting point reaches (4). At that point, you start iterating up the 4,3 line, until you reach point 3 with both starting points, and you're done.
*-------
\\\\\\\\ *
\\\\\\\\
*-----\\
------- *
Where the dashes are the starting points you calculated along 1-3 and 2-4, and the slashes are the fill lines.
Of course, this only works if the points are properly sorted, and you've got a convex polygon. If it's concave, you'll have to be very careful to not let your fill lines cross over the border, or do some pre-processing and subdivide the original poly into two or more convex ones.
You may want to look at Michael Abrash's articles on Dr Dobbs about polygon fill/raster/etc. It uses fixed-point math
Thomas, if you have a Bresenham line drawing algorithm available, then use it as a basis for further enhancement: divide your polygon to sub-polygons with an horizontal cutting line through every vertex. Then, start tracing the 2 left and right sides of each of these sub-polys, using Bresenham. This way you have the 2 end-points of each scan line in your polygon.
I would start by converting the polygon to a collection of triangles and render those, because triangles are easy to render by scanlines. Although even so there are some details.
Essentially, the draw-triangle sub-procedure will be given a raw triangle and proceed:
Reject degenerate triangles (where two of the three vertices overlap).
Sort the vertices in Y (since there are only three you can hardcode the sorting logic).
Now, at this point you should know that there will be three kinds of triangles: ones with a flat top, ones with a flat bottom, and "general" triangles. You want to handle a general triangle by essentially splitting it into one each of the flat types. This is because you don't want to have an if test every scanline to detect if the slope changed.
To render a flat triangle, you would run two Bresenham algorithms in parallel to iterate the pixels comprising the edges, and use the points they give you as the endpoints of each horizontal scanline.
It may be easier to break the problem into two parts. First, locate/write an algorithm that draws and fills a triangle. Second, write an algorithm that breaks up an arbitrary polygon into triangles (using different combinations of the vertices).
To draw/fill a triangle, use Bresenham's Line Algorithm to simultaneously draw a line between points 0 and 1, and between 1 and 2. For each input point x, draw the pixel if it is equal to or in between the y points generated by the two lines. When you reach one endpoint, continue by using the unfinished side and the side that has not yet been used.
Edit:
To break your convex polygon into triangles, arrange the points in order and call them P1, P2, ... PN. Let P1 be your "root" point, and build triangles using that point and combinations of adjacent points. For example, a pentagon would yield the three triangles P1-P2-P3, P1-P3-P4, and P1-P4-P5. In general, a convex polygon with N sides will decompose into N-2 triangles.