← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to mrol_mapping/bresenhamline.py

  • Committer: Julian Ryde
  • Date: 2012-03-04 02:43:54 UTC
  • mto: (16.1.12 trunk) (29.1.2 trunk)
  • mto: This revision was merged to the branch mainline in revision 25.
  • Revision ID: julian_ryde-20120304024354-yjf7woyr2bsd6vyn
Fixed mapper_test.  Updated the ground truth poses so that test passes aligning all simulated scans.

Show diffs side-by-side

added added

removed removed

Lines of Context:
mrol_mapping/bresenhamline.py
1
 
"""
2
 
N-D Bresenham line algo
3
 
"""
4
 
import numpy as np
5
 
def _bresenhamline_nslope(slope):
6
 
    """
7
 
    Normalize slope for Bresenham's line algorithm.
8
 
 
9
 
    >>> s = np.array([[-2, -2, -2, 0]])
10
 
    >>> _bresenhamline_nslope(s)
11
 
    array([[-1., -1., -1.,  0.]])
12
 
 
13
 
    >>> s = np.array([[0, 0, 0, 0]])
14
 
    >>> _bresenhamline_nslope(s)
15
 
    array([[ 0.,  0.,  0.,  0.]])
16
 
 
17
 
    >>> s = np.array([[0, 0, 9, 0]])
18
 
    >>> _bresenhamline_nslope(s)
19
 
    array([[ 0.,  0.,  1.,  0.]])
20
 
    """
21
 
    scale = np.amax(np.abs(slope), axis=1).reshape(-1, 1)
22
 
    zeroslope = (scale == 0).all(1)
23
 
    scale[zeroslope] = np.ones(1)
24
 
    normalizedslope = slope.astype(np.double) / scale
25
 
    normalizedslope[zeroslope] = np.zeros(slope[0].shape)
26
 
    return normalizedslope
27
 
 
28
 
def _bresenhamlines(start, end, max_iter):
29
 
    """
30
 
    Returns npts lines of length max_iter each. (npts x max_iter x dimension) 
31
 
 
32
 
    >>> s = np.array([[3, 1, 9, 0],[0, 0, 3, 0]])
33
 
    >>> _bresenhamlines(s, np.zeros(s.shape[1]), max_iter=-1)
34
 
    array([[[ 3,  1,  8,  0],
35
 
            [ 2,  1,  7,  0],
36
 
            [ 2,  1,  6,  0],
37
 
            [ 2,  1,  5,  0],
38
 
            [ 1,  0,  4,  0],
39
 
            [ 1,  0,  3,  0],
40
 
            [ 1,  0,  2,  0],
41
 
            [ 0,  0,  1,  0],
42
 
            [ 0,  0,  0,  0]],
43
 
    <BLANKLINE>
44
 
           [[ 0,  0,  2,  0],
45
 
            [ 0,  0,  1,  0],
46
 
            [ 0,  0,  0,  0],
47
 
            [ 0,  0, -1,  0],
48
 
            [ 0,  0, -2,  0],
49
 
            [ 0,  0, -3,  0],
50
 
            [ 0,  0, -4,  0],
51
 
            [ 0,  0, -5,  0],
52
 
            [ 0,  0, -6,  0]]])
53
 
    """
54
 
    if max_iter == -1:
55
 
        max_iter = np.amax(np.amax(np.abs(end - start), axis=1))
56
 
    npts, dim = start.shape
57
 
    nslope = _bresenhamline_nslope(end - start)
58
 
 
59
 
    # steps to iterate on
60
 
    stepseq = np.arange(1, max_iter + 1)
61
 
    stepmat = np.tile(stepseq, (dim, 1)).T
62
 
 
63
 
    # some hacks for broadcasting properly
64
 
    bline = start[:, np.newaxis, :] + nslope[:, np.newaxis, :] * stepmat
65
 
 
66
 
    # Approximate to nearest int
67
 
    return np.rint(bline).astype(start.dtype)
68
 
 
69
 
def bresenhamline(start, end, max_iter=5):
70
 
    """
71
 
    Returns a list of points from (start, end] by ray tracing a line b/w the
72
 
    points.
73
 
    Parameters:
74
 
        start: An array of start points (number of points x dimension)
75
 
        end:   An end points (1 x dimension)
76
 
            or An array of end point corresponding to each start point
77
 
                (number of points x dimension)
78
 
        max_iter: Max points to traverse. if -1, maximum number of required
79
 
                  points are traversed
80
 
 
81
 
    Returns:
82
 
        linevox (n x dimension) A cumulative array of all points traversed by
83
 
        all the lines so far.
84
 
 
85
 
    >>> s = np.array([[3, 1, 9, 0],[0, 0, 3, 0]])
86
 
    >>> bresenhamline(s, np.zeros(s.shape[1]), max_iter=-1)
87
 
    array([[ 3,  1,  8,  0],
88
 
           [ 2,  1,  7,  0],
89
 
           [ 2,  1,  6,  0],
90
 
           [ 2,  1,  5,  0],
91
 
           [ 1,  0,  4,  0],
92
 
           [ 1,  0,  3,  0],
93
 
           [ 1,  0,  2,  0],
94
 
           [ 0,  0,  1,  0],
95
 
           [ 0,  0,  0,  0],
96
 
           [ 0,  0,  2,  0],
97
 
           [ 0,  0,  1,  0],
98
 
           [ 0,  0,  0,  0],
99
 
           [ 0,  0, -1,  0],
100
 
           [ 0,  0, -2,  0],
101
 
           [ 0,  0, -3,  0],
102
 
           [ 0,  0, -4,  0],
103
 
           [ 0,  0, -5,  0],
104
 
           [ 0,  0, -6,  0]])
105
 
    """
106
 
    # Return the points as a single array
107
 
    return _bresenhamlines(start, end, max_iter).reshape(-1, start.shape[-1])
108
 
 
109
 
 
110
 
if __name__ == "__main__":
111
 
    import doctest
112
 
    doctest.testmod()