Merge lp:~wecacuee/mrol/mrol-dev into lp:~julian-ryde/mrol/main

[vikas]

1) Use backward ray tracing to remove noisy voxels upto a distance of 10 voxels
2) Vectorized support for userdata for colored voxels.
3) Added rviz as visualizer
4) Merged mrol_ros, the ros wrappers for using MROL.

[Julian Ryde (julian-ryde)]

review: Approve

Unmerged revisions

51. By Vikas Dhiman <email address hidden> on 2012-11-20

Merging lp:~wecacuee/mrol/mrol-dev

50. By Vikas Dhiman <email address hidden> on 2012-11-20

1) play_demo should also include the existing PYTHONPATH. 2) fixed volumetric_sample test case. The sampled points are guaranted to be within the resolution of original point but not at the center.

49. By Vikas Dhiman <email address hidden> on 2012-11-16

Manually added julians changes after merge failed. "Added function to save point cloud in the .ply format"; "Got speed_test working again after API changes.

48. By Vikas Dhiman <email address hidden> on 2012-11-16

Override julians repos with my version after merge

47. By Vikas Dhiman <email address hidden> on 2012-11-14

Removed unnecessary term from compile.sh

46. By vikas on 2012-06-29

Few more fixes. Do not overwrite estimated_trajectory.txt

45. By vikas on 2012-06-29

Fixing some fine grained test cases.

44. By vikas on 2012-05-22

Added support for userdata in terms of colored voxels. Added benchmark test and corresponding data

43. By vikas on 2012-05-14

Fixed a bug in generating image with gaussian noise.

42. By vikas on 2012-05-14

adding log.py

=== added file '.bzrignore'
--- .bzrignore	1970-01-01 00:00:00 +0000
+++ .bzrignore	2012-11-20 22:16:18 +0000
@@ -0,0 +1,11 @@
+build/
+dist/
+*.egg-info/
+.bzrignore
+data
+.syncme
+mapper_test.profile
+rviz.back.py
+TAGS
+tags
+*.glc

=== added file 'README'
--- README	1970-01-01 00:00:00 +0000
+++ README	2012-11-20 22:16:18 +0000
@@ -0,0 +1,52 @@
+DESCRIPTION
+
+A python library with two elements:
+
+The first is an efficient multi-resolution voxel representation for fast
+proximity determination. 
+
+The second is a global and local alignment algorithms for merging 
+multiple 3D point clouds into one map. Initial application is 3D mapping
+
+The algorithms are described in the following papers. Please cite 
+these in your work if you find this code useful.
+
+J. Ryde and H. Hu. 3D mapping with multi-resolution occupied voxel lists. Autonomous Robots, 2009.
+J. Ryde and N. Hillier. Alignment and 3D scene change detection for segmentation in autonomous earth moving. ICRA 2011.
+
+INSTALLATION
+
+Required packages (ubuntu/debian naming)
+
+python-nose python-matplotlib python-scipy python-numexpr python-visual python-numpy
+
+Optional packages
+python-coverage
+
+Make sure that the directory containing this README is on the PYTHONPATH
+
+To run tests:
+first compile with
+> ./compile.sh
+then execute tests with
+> ./run_tests
+
+COPYRIGHT
+
+Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
+
+WARRANTY
+
+This program is distributed in the hope that it will be useful,
+but WITHOUT ANY WARRANTY; without even the implied warranty of
+MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+GNU Lesser General Public License for more details.
+
+No obligations or promises are committed to for its maintenance. 
+
+LICENSING
+
+You should have received a copy of the GNU Lesser General Public License
+along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+

=== removed file 'README'
--- README	2011-11-29 22:17:03 +0000
+++ README	1970-01-01 00:00:00 +0000
@@ -1,52 +0,0 @@
-DESCRIPTION
-
-A python library with two elements:
-
-The first is an efficient multi-resolution voxel representation for fast
-proximity determination. 
-
-The second is a global and local alignment algorithms for merging 
-multiple 3D point clouds into one map. Initial application is 3D mapping
-
-The algorithms are described in the following papers. Please cite 
-these in your work if you find this code useful.
-
-J. Ryde and H. Hu. 3D mapping with multi-resolution occupied voxel lists. Autonomous Robots, 2009.
-J. Ryde and N. Hillier. Alignment and 3D scene change detection for segmentation in autonomous earth moving. ICRA 2011.
-
-INSTALLATION
-
-Required packages (ubuntu/debian naming)
-
-python-nose python-matplotlib python-scipy python-numexpr python-visual python-numpy
-
-Optional packages
-python-coverage
-
-Make sure that the directory containing this README is on the PYTHONPATH
-
-To run tests:
-first compile with
-> ./compile.sh
-then execute tests with
-> ./run_tests
-
-COPYRIGHT
-
-Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
-
-WARRANTY
-
-This program is distributed in the hope that it will be useful,
-but WITHOUT ANY WARRANTY; without even the implied warranty of
-MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-GNU Lesser General Public License for more details.
-
-No obligations or promises are committed to for its maintenance. 
-
-LICENSING
-
-You should have received a copy of the GNU Lesser General Public License
-along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-

=== added file 'compile.sh'
--- compile.sh	1970-01-01 00:00:00 +0000
+++ compile.sh	2012-11-20 22:16:18 +0000
@@ -0,0 +1,5 @@
+#! /bin/bash
+fname=`dirname $0`/mrol_mapping/cython/fast
+cython $fname.pyx
+#gcc -shared -pthread -fPIC -fwrapv -O3 -ffast-math -Wall -fno-strict-aliasing -I/usr/include/python2.6 -o $fname.so $fname.c
+gcc -shared -pthread -fPIC -fwrapv -O3 -Wall -fno-strict-aliasing -I/usr/include/python2.6 -o $fname.so $fname.c

=== removed file 'compile.sh'
--- compile.sh	2011-06-17 03:07:58 +0000
+++ compile.sh	1970-01-01 00:00:00 +0000
@@ -1,5 +0,0 @@
-#! /bin/bash
-fname=`dirname $0`/mrol_mapping/cython/fast
-cython $fname.pyx
-#gcc -shared -pthread -fPIC -fwrapv -O3 -ffast-math -Wall -fno-strict-aliasing -I/usr/include/python2.6 -o $fname.so $fname.c
-gcc -shared -pthread -fPIC -fwrapv -O3 -Wall -fno-strict-aliasing -I/usr/include/python2.6 -o $fname.so $fname.c

=== added directory 'experimental'
=== removed directory 'experimental'
=== added file 'experimental/cup_mapper.py'
--- experimental/cup_mapper.py	1970-01-01 00:00:00 +0000
+++ experimental/cup_mapper.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,99 @@
+'''File to build map from a directory of depth images and save to file'''
+from __future__ import division
+import mrol_mapping.poseutil as poseutil
+import mrol_mapping.mapper as mapper
+import mrol_mapping.pointcloud as pointcloud
+import mrol_mapping.util as util
+
+
+import mrol_mapping.occupiedlist as occupiedlist
+import mrol_mapping.quantizer as quantizer
+
+import os
+import numpy as np
+import sys
+
+sys.path.append('../test')
+import depth_image
+
+res = 0.01
+visualise = True
+make_free = False
+load_pc = True
+
+def get_pointcloud(fname):
+    xyzs = depth_image.image_to_points(fname)
+    pc = pointcloud.PointCloud(xyzs)
+    # remove ceiling and floor
+    pc.boxfilter(zmin=-15, zmax=0.5)
+    return pc
+    
+def get_freespace(xyzs, pose, voxelmap):
+    # TODO consider using mapper._build_sensor_model() and mapper.generate_freespace() instead.
+    # resampling to align with grid. bad hack, but makes ray_cast easier.
+    res = voxelmap.get_resolution()
+    xyzs = poseutil.transformpoints(xyzs,pose)
+    voxs = occupiedlist.pointstovoxels(xyzs, res)
+    voxs = quantizer.uniquerows(voxs, 0)
+    X = occupiedlist.voxelstopoints(voxs, res)
+    free_pts = voxelmap.free_space_ray_trace(X,pose[:3])
+    return free_pts
+        
+    
+
+if __name__ == '__main__':
+    try:
+        datadir = sys.argv[1]
+        outfname = sys.argv[2]+".map"
+        fnames = os.listdir(datadir)
+        fnames.sort()
+        if load_pc:
+            fnames = [datadir + '/' + fname for fname in fnames if fname.startswith('mrolpc')]
+        else:
+            fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')]
+    except:
+        print 'Need to specify a valid directory for input and file name for output'
+        sys.exit()
+
+    # variable initialisation
+    iros_map = mapper.VoxelMap(res,levels=3)
+    iros_free_map = mapper.VoxelMap(res,levels=1)
+    bestpose = poseutil.Pose3D()
+    
+    if load_pc:
+        pc_xform = poseutil.Pose3D(X=(0,0,0,0,0,0))
+        pc = pointcloud.load(fnames.pop(0))
+        pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
+        pc_xform = poseutil.Pose3D(X=(0,0,0,-np.pi/2.,0,-np.pi/2.))
+    else:
+        pc = get_pointcloud(fnames.pop(0))
+    iros_map.add_points(pc, bestpose)
+    if visualise:
+        iros_map.display(changes=False)
+    if make_free:
+        freepts = get_freespace(pc.points, bestpose, iros_map)
+        pcfree = pointcloud.PointCloud(freepts)
+        iros_free_map.add_points(pcfree,None)
+        if visualise:
+            iros_free_map.display(npts=1000000)
+
+    for fname in fnames:
+        print fname,
+        if load_pc:
+            pc = pointcloud.load(fname)
+            pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
+        else:
+            pc = get_pointcloud(fname)
+        bestpose = iros_map.align_points(pc, bestpose)[0]
+        iros_map.add_points(pc, bestpose)
+        if make_free:
+            freepts = get_freespace(pc.points, bestpose, iros_map)
+            pcfree = pointcloud.PointCloud(freepts)
+            iros_free_map.add_points(pcfree,None)
+        print bestpose
+
+    iros_map.save(outfname)
+    if make_free:
+        iros_free_map.save(outfname+"free")
+    print 'Map size:', len(iros_map), 'voxels.'
+    print 'Map saved to', outfname

=== removed file 'experimental/cup_mapper.py'
--- experimental/cup_mapper.py	2011-06-03 16:52:15 +0000
+++ experimental/cup_mapper.py	1970-01-01 00:00:00 +0000
@@ -1,99 +0,0 @@
-'''File to build map from a directory of depth images and save to file'''
-from __future__ import division
-import mrol_mapping.poseutil as poseutil
-import mrol_mapping.mapper as mapper
-import mrol_mapping.pointcloud as pointcloud
-import mrol_mapping.util as util
-
-
-import mrol_mapping.occupiedlist as occupiedlist
-import mrol_mapping.quantizer as quantizer
-
-import os
-import numpy as np
-import sys
-
-sys.path.append('../test')
-import depth_image
-
-res = 0.01
-visualise = True
-make_free = False
-load_pc = True
-
-def get_pointcloud(fname):
-    xyzs = depth_image.image_to_points(fname)
-    pc = pointcloud.PointCloud(xyzs)
-    # remove ceiling and floor
-    pc.boxfilter(zmin=-15, zmax=0.5)
-    return pc
-    
-def get_freespace(xyzs, pose, voxelmap):
-    # TODO consider using mapper._build_sensor_model() and mapper.generate_freespace() instead.
-    # resampling to align with grid. bad hack, but makes ray_cast easier.
-    res = voxelmap.get_resolution()
-    xyzs = poseutil.transformpoints(xyzs,pose)
-    voxs = occupiedlist.pointstovoxels(xyzs, res)
-    voxs = quantizer.uniquerows(voxs, 0)
-    X = occupiedlist.voxelstopoints(voxs, res)
-    free_pts = voxelmap.free_space_ray_trace(X,pose[:3])
-    return free_pts
-        
-    
-
-if __name__ == '__main__':
-    try:
-        datadir = sys.argv[1]
-        outfname = sys.argv[2]+".map"
-        fnames = os.listdir(datadir)
-        fnames.sort()
-        if load_pc:
-            fnames = [datadir + '/' + fname for fname in fnames if fname.startswith('mrolpc')]
-        else:
-            fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')]
-    except:
-        print 'Need to specify a valid directory for input and file name for output'
-        sys.exit()
-
-    # variable initialisation
-    iros_map = mapper.VoxelMap(res,levels=3)
-    iros_free_map = mapper.VoxelMap(res,levels=1)
-    bestpose = poseutil.Pose3D()
-    
-    if load_pc:
-        pc_xform = poseutil.Pose3D(X=(0,0,0,0,0,0))
-        pc = pointcloud.load(fnames.pop(0))
-        pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
-        pc_xform = poseutil.Pose3D(X=(0,0,0,-np.pi/2.,0,-np.pi/2.))
-    else:
-        pc = get_pointcloud(fnames.pop(0))
-    iros_map.add_points(pc, bestpose)
-    if visualise:
-        iros_map.display(changes=False)
-    if make_free:
-        freepts = get_freespace(pc.points, bestpose, iros_map)
-        pcfree = pointcloud.PointCloud(freepts)
-        iros_free_map.add_points(pcfree,None)
-        if visualise:
-            iros_free_map.display(npts=1000000)
-
-    for fname in fnames:
-        print fname,
-        if load_pc:
-            pc = pointcloud.load(fname)
-            pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
-        else:
-            pc = get_pointcloud(fname)
-        bestpose = iros_map.align_points(pc, bestpose)[0]
-        iros_map.add_points(pc, bestpose)
-        if make_free:
-            freepts = get_freespace(pc.points, bestpose, iros_map)
-            pcfree = pointcloud.PointCloud(freepts)
-            iros_free_map.add_points(pcfree,None)
-        print bestpose
-
-    iros_map.save(outfname)
-    if make_free:
-        iros_free_map.save(outfname+"free")
-    print 'Map size:', len(iros_map), 'voxels.'
-    print 'Map saved to', outfname

=== added file 'experimental/cup_segmenter.py'
--- experimental/cup_segmenter.py	1970-01-01 00:00:00 +0000
+++ experimental/cup_segmenter.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,142 @@
+import sys
+import numpy as np
+
+import mrol_mapping.mapper as mapper
+import mrol_mapping.poseutil as poseutil
+import mrol_mapping.occupiedlist as occupiedlist
+import mrol_mapping.pointcloud as pointcloud
+import mrol_mapping.visualiser.robotvisualiser as robotvisualiser
+import mrol_mapping.quantizer as quantizer
+import os
+
+sys.path.append('../test')
+import iros_mapper
+
+
+import signal
+def signal_handler(signal, frame):
+    import pdb; pdb.set_trace()
+signal.signal(signal.SIGINT, signal_handler)
+
+#fname =  '/jcr/data3d/lamp/present/0002.png'
+load_pc = True# for loading point clouds from the Kinect from npy files
+try:
+    datadir = sys.argv[1]
+    fnames = os.listdir(datadir)
+    fnames.sort()
+    if load_pc:
+        fnames = [datadir + '/' + fname for fname in fnames if fname.startswith('mrolpc')]
+    else:
+        fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')]
+except:
+    print 'Need to specify a valid directory for input'
+    sys.exit()
+visualise = True
+long_term = False
+
+   
+bestpose = poseutil.Pose3D(X=(0,0,0,0,0,0))
+pc_xform = poseutil.Pose3D(X=(0,0,0,-np.pi/2.,0,-np.pi/2.))
+res = 0.01
+segment_map = mapper.VoxelMap(res,levels=3)
+pc = pointcloud.load(fnames.pop(0))
+#pc = pointcloud.PointCloud(pc)
+pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
+segment_map.add_points(pc, bestpose)
+object_map = mapper.VoxelMap(res/4.,levels=1)
+
+
+#freespace_ol = segment_map.generate_freespace2(resolution = res*2, minR=0.4, maxR=3)
+#freespace_ol = segment_map.generate_freespace(res, minR=0.25, maxR=0.75) # *4 for speed during testing, should just be res
+
+free_space_res = res
+
+pc_vox = occupiedlist.pointstovoxels(pc.points, free_space_res)
+pc_vox = quantizer.uniquerows(pc_vox, 0)
+pc_pts = occupiedlist.voxelstopoints(pc_vox, free_space_res)
+pc_regular = pointcloud.PointCloud(pc_pts)
+freespace = segment_map.free_space_ray_trace(pc_regular,(0,0,0),free_space_res,voxel_offset=2.5)
+freespace_ol = occupiedlist.OccupiedList(free_space_res)
+freespace_ol.add_points(freespace.points)
+
+#vis = robotvisualiser.Visualiser(npts=2e6)
+#vis.addmappts(segment_map.get_points().points)
+#vis.addmappts(freespace_ol.getpoints())
+#free_pc = pointcloud.PointCloud(freespace_ol.getpoints())
+#free_pc.display()
+
+
+
+if visualise:
+    vis = robotvisualiser.Visualiser(npts=2e6)
+    vis.set_orthographic(Bool=True)
+    vis.setminmax(-1.5,1.5)
+    #objvis = robotvisualiser.Visualiser()
+    #objvis.set_orthographic(Bool=True)
+    #objvis.setminmax(-1.5,1.5)
+
+pc_xform = poseutil.Pose3D(X=(0,0,0,-np.pi/2.,0,-np.pi/2.))
+count = 0
+for fname in fnames:
+    if load_pc:
+        pc = pointcloud.load(fname)
+        pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
+    else:
+        pc = get_pointcloud(fname)
+    print fname,
+    bestpose = segment_map.align_points(pc, bestpose)[0]
+    print bestpose
+    hits, new_points  = segment_map.mrol.getfinest().segment_ovl(bestpose,pc.points)
+    if long_term:
+        # if we wanted a long-term map without dynamic object coruption 
+        inliers, outliers = freespace_ol.segment_ovl(None, new_points)
+        object_map.add_points(pointcloud.PointCloud(inliers), None)
+        segment_map.add_points(pointcloud.PointCloud(outliers), None)
+         #if freespace2:
+        #    if len(inliers) > 0:
+        #        tmp_freespace_ol = segment_map.generate_freespace2(inliers,pose=bestpose)
+        #    else:
+        #        # nothing to segment?
+        #        tmp_freespace_ol = occupiedlist.OccupiedList(1)
+        #else:
+        #    tmp_freespace_ol = segment_map.generate_freespace(res,pose=bestpose)
+        #
+        #freespace_ol.add_points(tmp_freespace_ol.getpoints(), None)
+    else:
+        # if we are just trying to segment out the cup to make a model, and it's not moving.
+        inliers, outliers = freespace_ol.segment_ovl(bestpose,pc.points)
+        #TODO only add points if overlap is good?
+        if count == 0:
+            object_map.add_points(pointcloud.PointCloud(inliers), None)
+            segment_map.add_points(pointcloud.PointCloud(new_points), None)
+        else:
+            if len(inliers)/len(object_map) > 0.5:
+                object_map.add_points(pointcloud.PointCloud(inliers), None)
+                segment_map.add_points(pointcloud.PointCloud(new_points), None)
+            else: 
+                print "Not adding to map"
+
+    if visualise:
+        print "Visualising"
+        vis.clear()
+        vis.addmappts(segment_map.get_points().points)
+        vis.setrobotpose(bestpose.getMatrix())
+        vis.addtrajectorypoint(bestpose.getTuple()[0:3])
+        if long_term:
+            if len(outliers) > 0:
+                vis.setrightpts(outliers) # new map points
+        else:
+            vis.setrightpts(new_points)
+        if len(inliers) > 0:
+            vis.setleftpts(inliers) # segmented points
+            #pointcloud.save(fname+"_seg",inliers)
+            #np.save(fname+"_pose",np.asarray(bestpose.getTuple()))
+        
+        #objvis.clear()
+        #objvis.addmappts(object_map.get_points().points)
+        #free_pc = pointcloud.PointCloud(freespace_ol.getpoints())
+        #P = free_pc.display()
+    count +=1
+import pdb;pdb.set_trace()
+segment_map.save("segment_map.map")
+object_map.save("object_map.map")

=== removed file 'experimental/cup_segmenter.py'
--- experimental/cup_segmenter.py	2012-02-26 19:51:50 +0000
+++ experimental/cup_segmenter.py	1970-01-01 00:00:00 +0000
@@ -1,142 +0,0 @@
-import sys
-import numpy as np
-
-import mrol_mapping.mapper as mapper
-import mrol_mapping.poseutil as poseutil
-import mrol_mapping.occupiedlist as occupiedlist
-import mrol_mapping.pointcloud as pointcloud
-import mrol_mapping.visualiser.robotvisualiser as robotvisualiser
-import mrol_mapping.quantizer as quantizer
-import os
-
-sys.path.append('../test')
-import iros_mapper
-
-
-import signal
-def signal_handler(signal, frame):
-    import pdb; pdb.set_trace()
-signal.signal(signal.SIGINT, signal_handler)
-
-#fname =  '/jcr/data3d/lamp/present/0002.png'
-load_pc = True# for loading point clouds from the Kinect from npy files
-try:
-    datadir = sys.argv[1]
-    fnames = os.listdir(datadir)
-    fnames.sort()
-    if load_pc:
-        fnames = [datadir + '/' + fname for fname in fnames if fname.startswith('mrolpc')]
-    else:
-        fnames = [datadir + '/' + fname for fname in fnames if fname.endswith('.png')]
-except:
-    print 'Need to specify a valid directory for input'
-    sys.exit()
-visualise = True
-long_term = False
-
-   
-bestpose = poseutil.Pose3D(X=(0,0,0,0,0,0))
-pc_xform = poseutil.Pose3D(X=(0,0,0,-np.pi/2.,0,-np.pi/2.))
-res = 0.01
-segment_map = mapper.VoxelMap(res,levels=3)
-pc = pointcloud.load(fnames.pop(0))
-#pc = pointcloud.PointCloud(pc)
-pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
-segment_map.add_points(pc, bestpose)
-object_map = mapper.VoxelMap(res/4.,levels=1)
-
-
-#freespace_ol = segment_map.generate_freespace2(resolution = res*2, minR=0.4, maxR=3)
-#freespace_ol = segment_map.generate_freespace(res, minR=0.25, maxR=0.75) # *4 for speed during testing, should just be res
-
-free_space_res = res
-
-pc_vox = occupiedlist.pointstovoxels(pc.points, free_space_res)
-pc_vox = quantizer.uniquerows(pc_vox, 0)
-pc_pts = occupiedlist.voxelstopoints(pc_vox, free_space_res)
-pc_regular = pointcloud.PointCloud(pc_pts)
-freespace = segment_map.free_space_ray_trace(pc_regular,(0,0,0),free_space_res,voxel_offset=2.5)
-freespace_ol = occupiedlist.OccupiedList(free_space_res)
-freespace_ol.add_points(freespace.points)
-
-#vis = robotvisualiser.Visualiser(npts=2e6)
-#vis.addmappts(segment_map.get_points().points)
-#vis.addmappts(freespace_ol.getpoints())
-#free_pc = pointcloud.PointCloud(freespace_ol.getpoints())
-#free_pc.display()
-
-
-
-if visualise:
-    vis = robotvisualiser.Visualiser(npts=2e6)
-    vis.set_orthographic(Bool=True)
-    vis.setminmax(-1.5,1.5)
-    #objvis = robotvisualiser.Visualiser()
-    #objvis.set_orthographic(Bool=True)
-    #objvis.setminmax(-1.5,1.5)
-
-pc_xform = poseutil.Pose3D(X=(0,0,0,-np.pi/2.,0,-np.pi/2.))
-count = 0
-for fname in fnames:
-    if load_pc:
-        pc = pointcloud.load(fname)
-        pc = pointcloud.PointCloud(pc_xform.transformPoints(pc))
-    else:
-        pc = get_pointcloud(fname)
-    print fname,
-    bestpose = segment_map.align_points(pc, bestpose)[0]
-    print bestpose
-    hits, new_points  = segment_map.mrol.getfinest().segment_ovl(bestpose,pc.points)
-    if long_term:
-        # if we wanted a long-term map without dynamic object coruption 
-        inliers, outliers = freespace_ol.segment_ovl(None, new_points)
-        object_map.add_points(pointcloud.PointCloud(inliers), None)
-        segment_map.add_points(pointcloud.PointCloud(outliers), None)
-         #if freespace2:
-        #    if len(inliers) > 0:
-        #        tmp_freespace_ol = segment_map.generate_freespace2(inliers,pose=bestpose)
-        #    else:
-        #        # nothing to segment?
-        #        tmp_freespace_ol = occupiedlist.OccupiedList(1)
-        #else:
-        #    tmp_freespace_ol = segment_map.generate_freespace(res,pose=bestpose)
-        #
-        #freespace_ol.add_points(tmp_freespace_ol.getpoints(), None)
-    else:
-        # if we are just trying to segment out the cup to make a model, and it's not moving.
-        inliers, outliers = freespace_ol.segment_ovl(bestpose,pc.points)
-        #TODO only add points if overlap is good?
-        if count == 0:
-            object_map.add_points(pointcloud.PointCloud(inliers), None)
-            segment_map.add_points(pointcloud.PointCloud(new_points), None)
-        else:
-            if len(inliers)/len(object_map) > 0.5:
-                object_map.add_points(pointcloud.PointCloud(inliers), None)
-                segment_map.add_points(pointcloud.PointCloud(new_points), None)
-            else: 
-                print "Not adding to map"
-
-    if visualise:
-        print "Visualising"
-        vis.clear()
-        vis.addmappts(segment_map.get_points().points)
-        vis.setrobotpose(bestpose.getMatrix())
-        vis.addtrajectorypoint(bestpose.getTuple()[0:3])
-        if long_term:
-            if len(outliers) > 0:
-                vis.setrightpts(outliers) # new map points
-        else:
-            vis.setrightpts(new_points)
-        if len(inliers) > 0:
-            vis.setleftpts(inliers) # segmented points
-            #pointcloud.save(fname+"_seg",inliers)
-            #np.save(fname+"_pose",np.asarray(bestpose.getTuple()))
-        
-        #objvis.clear()
-        #objvis.addmappts(object_map.get_points().points)
-        #free_pc = pointcloud.PointCloud(freespace_ol.getpoints())
-        #P = free_pc.display()
-    count +=1
-import pdb;pdb.set_trace()
-segment_map.save("segment_map.map")
-object_map.save("object_map.map")

=== added file 'full_test.sh'
--- full_test.sh	1970-01-01 00:00:00 +0000
+++ full_test.sh	2012-11-20 22:16:18 +0000
@@ -0,0 +1,9 @@
+#! /bin/sh
+# does a fresh branch of the local repo in tmp, compiles and tests
+# commit locally and run this before pushing to the main repository
+rm -rf /tmp/mrol_repo
+bzr branch . /tmp/mrol_repo
+cd /tmp/mrol_repo
+#./compile.sh
+./run_tests
+rm -rf /tmp/mrol_repo

=== removed file 'full_test.sh'
--- full_test.sh	2011-11-29 22:19:46 +0000
+++ full_test.sh	1970-01-01 00:00:00 +0000
@@ -1,9 +0,0 @@
-#! /bin/sh
-# does a fresh branch of the local repo in tmp, compiles and tests
-# commit locally and run this before pushing to the main repository
-rm -rf /tmp/mrol_repo
-bzr branch . /tmp/mrol_repo
-cd /tmp/mrol_repo
-#./compile.sh
-./run_tests
-rm -rf /tmp/mrol_repo

=== added file 'lgpl.txt'
--- lgpl.txt	1970-01-01 00:00:00 +0000
+++ lgpl.txt	2012-11-20 22:16:18 +0000
@@ -0,0 +1,166 @@
+                   GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+  This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+  0. Additional Definitions.
+
+  As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+  "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+  An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+  A "Combined Work" is a work produced by combining or linking an
+Application with the Library.  The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+  The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+  The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+  1. Exception to Section 3 of the GNU GPL.
+
+  You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+  2. Conveying Modified Versions.
+
+  If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+   a) under this License, provided that you make a good faith effort to
+   ensure that, in the event an Application does not supply the
+   function or data, the facility still operates, and performs
+   whatever part of its purpose remains meaningful, or
+
+   b) under the GNU GPL, with none of the additional permissions of
+   this License applicable to that copy.
+
+  3. Object Code Incorporating Material from Library Header Files.
+
+  The object code form of an Application may incorporate material from
+a header file that is part of the Library.  You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+   a) Give prominent notice with each copy of the object code that the
+   Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the object code with a copy of the GNU GPL and this license
+   document.
+
+  4. Combined Works.
+
+  You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+   a) Give prominent notice with each copy of the Combined Work that
+   the Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the Combined Work with a copy of the GNU GPL and this license
+   document.
+
+   c) For a Combined Work that displays copyright notices during
+   execution, include the copyright notice for the Library among
+   these notices, as well as a reference directing the user to the
+   copies of the GNU GPL and this license document.
+
+   d) Do one of the following:
+
+       0) Convey the Minimal Corresponding Source under the terms of this
+       License, and the Corresponding Application Code in a form
+       suitable for, and under terms that permit, the user to
+       recombine or relink the Application with a modified version of
+       the Linked Version to produce a modified Combined Work, in the
+       manner specified by section 6 of the GNU GPL for conveying
+       Corresponding Source.
+
+       1) Use a suitable shared library mechanism for linking with the
+       Library.  A suitable mechanism is one that (a) uses at run time
+       a copy of the Library already present on the user's computer
+       system, and (b) will operate properly with a modified version
+       of the Library that is interface-compatible with the Linked
+       Version.
+
+   e) Provide Installation Information, but only if you would otherwise
+   be required to provide such information under section 6 of the
+   GNU GPL, and only to the extent that such information is
+   necessary to install and execute a modified version of the
+   Combined Work produced by recombining or relinking the
+   Application with a modified version of the Linked Version. (If
+   you use option 4d0, the Installation Information must accompany
+   the Minimal Corresponding Source and Corresponding Application
+   Code. If you use option 4d1, you must provide the Installation
+   Information in the manner specified by section 6 of the GNU GPL
+   for conveying Corresponding Source.)
+
+  5. Combined Libraries.
+
+  You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+   a) Accompany the combined library with a copy of the same work based
+   on the Library, uncombined with any other library facilities,
+   conveyed under the terms of this License.
+
+   b) Give prominent notice with the combined library that part of it
+   is a work based on the Library, and explaining where to find the
+   accompanying uncombined form of the same work.
+
+  6. Revised Versions of the GNU Lesser General Public License.
+
+  The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+  Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+  If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.
+

=== removed file 'lgpl.txt'
--- lgpl.txt	2011-07-05 06:26:34 +0000
+++ lgpl.txt	1970-01-01 00:00:00 +0000
@@ -1,166 +0,0 @@
-                   GNU LESSER GENERAL PUBLIC LICENSE
-                       Version 3, 29 June 2007
-
- Copyright (C) 2007 Free Software Foundation, Inc. <http://fsf.org/>
- Everyone is permitted to copy and distribute verbatim copies
- of this license document, but changing it is not allowed.
-
-
-  This version of the GNU Lesser General Public License incorporates
-the terms and conditions of version 3 of the GNU General Public
-License, supplemented by the additional permissions listed below.
-
-  0. Additional Definitions.
-
-  As used herein, "this License" refers to version 3 of the GNU Lesser
-General Public License, and the "GNU GPL" refers to version 3 of the GNU
-General Public License.
-
-  "The Library" refers to a covered work governed by this License,
-other than an Application or a Combined Work as defined below.
-
-  An "Application" is any work that makes use of an interface provided
-by the Library, but which is not otherwise based on the Library.
-Defining a subclass of a class defined by the Library is deemed a mode
-of using an interface provided by the Library.
-
-  A "Combined Work" is a work produced by combining or linking an
-Application with the Library.  The particular version of the Library
-with which the Combined Work was made is also called the "Linked
-Version".
-
-  The "Minimal Corresponding Source" for a Combined Work means the
-Corresponding Source for the Combined Work, excluding any source code
-for portions of the Combined Work that, considered in isolation, are
-based on the Application, and not on the Linked Version.
-
-  The "Corresponding Application Code" for a Combined Work means the
-object code and/or source code for the Application, including any data
-and utility programs needed for reproducing the Combined Work from the
-Application, but excluding the System Libraries of the Combined Work.
-
-  1. Exception to Section 3 of the GNU GPL.
-
-  You may convey a covered work under sections 3 and 4 of this License
-without being bound by section 3 of the GNU GPL.
-
-  2. Conveying Modified Versions.
-
-  If you modify a copy of the Library, and, in your modifications, a
-facility refers to a function or data to be supplied by an Application
-that uses the facility (other than as an argument passed when the
-facility is invoked), then you may convey a copy of the modified
-version:
-
-   a) under this License, provided that you make a good faith effort to
-   ensure that, in the event an Application does not supply the
-   function or data, the facility still operates, and performs
-   whatever part of its purpose remains meaningful, or
-
-   b) under the GNU GPL, with none of the additional permissions of
-   this License applicable to that copy.
-
-  3. Object Code Incorporating Material from Library Header Files.
-
-  The object code form of an Application may incorporate material from
-a header file that is part of the Library.  You may convey such object
-code under terms of your choice, provided that, if the incorporated
-material is not limited to numerical parameters, data structure
-layouts and accessors, or small macros, inline functions and templates
-(ten or fewer lines in length), you do both of the following:
-
-   a) Give prominent notice with each copy of the object code that the
-   Library is used in it and that the Library and its use are
-   covered by this License.
-
-   b) Accompany the object code with a copy of the GNU GPL and this license
-   document.
-
-  4. Combined Works.
-
-  You may convey a Combined Work under terms of your choice that,
-taken together, effectively do not restrict modification of the
-portions of the Library contained in the Combined Work and reverse
-engineering for debugging such modifications, if you also do each of
-the following:
-
-   a) Give prominent notice with each copy of the Combined Work that
-   the Library is used in it and that the Library and its use are
-   covered by this License.
-
-   b) Accompany the Combined Work with a copy of the GNU GPL and this license
-   document.
-
-   c) For a Combined Work that displays copyright notices during
-   execution, include the copyright notice for the Library among
-   these notices, as well as a reference directing the user to the
-   copies of the GNU GPL and this license document.
-
-   d) Do one of the following:
-
-       0) Convey the Minimal Corresponding Source under the terms of this
-       License, and the Corresponding Application Code in a form
-       suitable for, and under terms that permit, the user to
-       recombine or relink the Application with a modified version of
-       the Linked Version to produce a modified Combined Work, in the
-       manner specified by section 6 of the GNU GPL for conveying
-       Corresponding Source.
-
-       1) Use a suitable shared library mechanism for linking with the
-       Library.  A suitable mechanism is one that (a) uses at run time
-       a copy of the Library already present on the user's computer
-       system, and (b) will operate properly with a modified version
-       of the Library that is interface-compatible with the Linked
-       Version.
-
-   e) Provide Installation Information, but only if you would otherwise
-   be required to provide such information under section 6 of the
-   GNU GPL, and only to the extent that such information is
-   necessary to install and execute a modified version of the
-   Combined Work produced by recombining or relinking the
-   Application with a modified version of the Linked Version. (If
-   you use option 4d0, the Installation Information must accompany
-   the Minimal Corresponding Source and Corresponding Application
-   Code. If you use option 4d1, you must provide the Installation
-   Information in the manner specified by section 6 of the GNU GPL
-   for conveying Corresponding Source.)
-
-  5. Combined Libraries.
-
-  You may place library facilities that are a work based on the
-Library side by side in a single library together with other library
-facilities that are not Applications and are not covered by this
-License, and convey such a combined library under terms of your
-choice, if you do both of the following:
-
-   a) Accompany the combined library with a copy of the same work based
-   on the Library, uncombined with any other library facilities,
-   conveyed under the terms of this License.
-
-   b) Give prominent notice with the combined library that part of it
-   is a work based on the Library, and explaining where to find the
-   accompanying uncombined form of the same work.
-
-  6. Revised Versions of the GNU Lesser General Public License.
-
-  The Free Software Foundation may publish revised and/or new versions
-of the GNU Lesser General Public License from time to time. Such new
-versions will be similar in spirit to the present version, but may
-differ in detail to address new problems or concerns.
-
-  Each version is given a distinguishing version number. If the
-Library as you received it specifies that a certain numbered version
-of the GNU Lesser General Public License "or any later version"
-applies to it, you have the option of following the terms and
-conditions either of that published version or of any later version
-published by the Free Software Foundation. If the Library as you
-received it does not specify a version number of the GNU Lesser
-General Public License, you may choose any version of the GNU Lesser
-General Public License ever published by the Free Software Foundation.
-
-  If the Library as you received it specifies that a proxy can decide
-whether future versions of the GNU Lesser General Public License shall
-apply, that proxy's public statement of acceptance of any version is
-permanent authorization for you to choose that version for the
-Library.
-

=== added directory 'mrol_mapping'
=== removed directory 'mrol_mapping'
=== added file 'mrol_mapping/__init__.py'
--- mrol_mapping/__init__.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/__init__.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,2 @@
+import os
+root_dir = os.path.abspath(os.path.dirname(__file__))

=== removed file 'mrol_mapping/__init__.py'
--- mrol_mapping/__init__.py	2011-06-03 16:52:15 +0000
+++ mrol_mapping/__init__.py	1970-01-01 00:00:00 +0000
@@ -1,2 +0,0 @@
-import os
-root_dir = os.path.abspath(os.path.dirname(__file__))

=== added file 'mrol_mapping/bresenhamline.py'
--- mrol_mapping/bresenhamline.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/bresenhamline.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,112 @@
+"""
+N-D Bresenham line algo
+"""
+import numpy as np
+def _bresenhamline_nslope(slope):
+    """
+    Normalize slope for Bresenham's line algorithm.
+
+    >>> s = np.array([[-2, -2, -2, 0]])
+    >>> _bresenhamline_nslope(s)
+    array([[-1., -1., -1.,  0.]])
+
+    >>> s = np.array([[0, 0, 0, 0]])
+    >>> _bresenhamline_nslope(s)
+    array([[ 0.,  0.,  0.,  0.]])
+
+    >>> s = np.array([[0, 0, 9, 0]])
+    >>> _bresenhamline_nslope(s)
+    array([[ 0.,  0.,  1.,  0.]])
+    """
+    scale = np.amax(np.abs(slope), axis=1).reshape(-1, 1)
+    zeroslope = (scale == 0).all(1)
+    scale[zeroslope] = np.ones(1)
+    normalizedslope = slope.astype(np.double) / scale
+    normalizedslope[zeroslope] = np.zeros(slope[0].shape)
+    return normalizedslope
+
+def _bresenhamlines(start, end, max_iter):
+    """
+    Returns npts lines of length max_iter each. (npts x max_iter x dimension) 
+
+    >>> s = np.array([[3, 1, 9, 0],[0, 0, 3, 0]])
+    >>> _bresenhamlines(s, np.zeros(s.shape[1]), max_iter=-1)
+    array([[[ 3,  1,  8,  0],
+            [ 2,  1,  7,  0],
+            [ 2,  1,  6,  0],
+            [ 2,  1,  5,  0],
+            [ 1,  0,  4,  0],
+            [ 1,  0,  3,  0],
+            [ 1,  0,  2,  0],
+            [ 0,  0,  1,  0],
+            [ 0,  0,  0,  0]],
+    <BLANKLINE>
+           [[ 0,  0,  2,  0],
+            [ 0,  0,  1,  0],
+            [ 0,  0,  0,  0],
+            [ 0,  0, -1,  0],
+            [ 0,  0, -2,  0],
+            [ 0,  0, -3,  0],
+            [ 0,  0, -4,  0],
+            [ 0,  0, -5,  0],
+            [ 0,  0, -6,  0]]])
+    """
+    if max_iter == -1:
+        max_iter = np.amax(np.amax(np.abs(end - start), axis=1))
+    npts, dim = start.shape
+    nslope = _bresenhamline_nslope(end - start)
+
+    # steps to iterate on
+    stepseq = np.arange(1, max_iter + 1)
+    stepmat = np.tile(stepseq, (dim, 1)).T
+
+    # some hacks for broadcasting properly
+    bline = start[:, np.newaxis, :] + nslope[:, np.newaxis, :] * stepmat
+
+    # Approximate to nearest int
+    return np.rint(bline).astype(start.dtype)
+
+def bresenhamline(start, end, max_iter=5):
+    """
+    Returns a list of points from (start, end] by ray tracing a line b/w the
+    points.
+    Parameters:
+        start: An array of start points (number of points x dimension)
+        end:   An end points (1 x dimension)
+            or An array of end point corresponding to each start point
+                (number of points x dimension)
+        max_iter: Max points to traverse. if -1, maximum number of required
+                  points are traversed
+
+    Returns:
+        linevox (n x dimension) A cumulative array of all points traversed by
+        all the lines so far.
+
+    >>> s = np.array([[3, 1, 9, 0],[0, 0, 3, 0]])
+    >>> bresenhamline(s, np.zeros(s.shape[1]), max_iter=-1)
+    array([[ 3,  1,  8,  0],
+           [ 2,  1,  7,  0],
+           [ 2,  1,  6,  0],
+           [ 2,  1,  5,  0],
+           [ 1,  0,  4,  0],
+           [ 1,  0,  3,  0],
+           [ 1,  0,  2,  0],
+           [ 0,  0,  1,  0],
+           [ 0,  0,  0,  0],
+           [ 0,  0,  2,  0],
+           [ 0,  0,  1,  0],
+           [ 0,  0,  0,  0],
+           [ 0,  0, -1,  0],
+           [ 0,  0, -2,  0],
+           [ 0,  0, -3,  0],
+           [ 0,  0, -4,  0],
+           [ 0,  0, -5,  0],
+           [ 0,  0, -6,  0]])
+    """
+    # Return the points as a single array
+    return _bresenhamlines(start, end, max_iter).reshape(-1, start.shape[-1])
+
+
+if __name__ == "__main__":
+    import doctest
+    doctest.testmod()

=== added directory 'mrol_mapping/cython'
=== removed directory 'mrol_mapping/cython'
=== added file 'mrol_mapping/cython/__init__.py'
=== removed file 'mrol_mapping/cython/__init__.py'
=== added file 'mrol_mapping/cython/fast.pyx'
--- mrol_mapping/cython/fast.pyx	1970-01-01 00:00:00 +0000
+++ mrol_mapping/cython/fast.pyx	2012-11-20 22:16:18 +0000
@@ -0,0 +1,113 @@
+#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
+#
+#This program is distributed in the hope that it will be useful,
+#but WITHOUT ANY WARRANTY; without even the implied warranty of
+#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#GNU Lesser General Public License for more details.
+#
+#You should have received a copy of the GNU Lesser General Public License
+#along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+
+cimport cython
+import numpy as np
+cimport numpy as np
+
+#ctypedef np.float64_t dtype_t
+# difference between 32 and 64 bit
+ctypedef np.int64_t dtype_i
+#ctypedef np.int32_t dtype_i
+
+#cdef dict a
+#a = {}
+#a['a'] = 1
+
[e-mail](False)
+def intersection(voxelmap, np.ndarray[dtype_i, ndim=1] voxels):
+    cdef Py_ssize_t i, overlap = 0
+    for i in range(voxels.shape[0]):
+        if voxels[i] in voxelmap:
+            overlap += 1
+    return overlap
+
[e-mail](False)
+#@cython.locals(a=cython.double, b=cython.double,
+               #N=cython.Py_ssize_t, dx=cython.double,
+               #s=cython.double, i=cython.Py_ssize_t)
+#@cython.locals(a=cython.double, b=cython.double,
+def nested_intersection(dict D, np.ndarray[dtype_i, ndim=2] V):
+    cdef Py_ssize_t i, a, b, c, overlap = 0
+    for a, b, c in V:
+        if a in D and b in D[a] and c in D[a][b]:
+            overlap += 1
+    return overlap
+    
+#@cython.boundscheck(False)
+#def intersection(voxelmap, np.ndarray[dtype_i, ndim=2] voxels):
+#    cdef Py_ssize_t i, overlap = 0
+#    for i in range(voxels.shape[0]):
+#        if (voxels[i, 0], voxels[i, 1], voxels[i, 2]) in voxelmap:
+#            overlap += 1
+#    return overlap
+
[e-mail](False)
+def fillray(dtype_i numcells, np.ndarray[np.float64_t, ndim=1] dir_unit, np.float64_t res, np.ndarray[np.float64_t, ndim=2] ray):
+    cdef int i
+    for i in range(numcells):
+        ray[i] = dir_unit*i*res
+    return ray
+    
[e-mail](False)
+def increment(dict mapvoxels, np.ndarray[dtype_i, ndim=2] voxels, dtype_i amount=1, userdata=None):
+    # TODO ensure this remains consistend with the decrement function
+    # TODO fails when there is only one voxel
+    # TODO add ability to increment according to a vector of occupancies same 
+    # length as the array of voxels ie amount should also be allowed to be a 
+    # vector of occupancies
+    if amount < 0:
+        print "Warning: negative increment amount. Consider using decrement() instead."
+    cdef np.ndarray[dtype_i, ndim=1] newvoxels = np.zeros(voxels.shape[0], dtype=np.int64)
+    cdef dtype_i v0, v1, v2, occupancy
+    cdef tuple v
+    for i in range(voxels.shape[0]):
+        occupancy = 0
+        v0 = voxels[i,0]
+        v1 = voxels[i,1]
+        v2 = voxels[i,2]
+        v = (v0, v1, v2) # ensure v is a tuple
+        if mapvoxels.has_key(v):
+            newvoxels[i] = 0
+            occupancy = mapvoxels[v] + amount
+        else:
+            newvoxels[i] = 1
+            occupancy = amount
+        if occupancy > 0:
+            if userdata == None:
+                mapvoxels[v] = occupancy
+            else:
+                mapvoxels[v] = (occupancy, userdata)
+        else:
+            del mapvoxels[v]
+    return newvoxels.astype(np.bool)
+    
+
[e-mail](False)
+def user_increment(mapvoxels, np.ndarray[np.int_t, ndim=1] ids, np.ndarray[np.float64_t, ndim=1] increments, userdata, useraddfn):
+    cdef np.ndarray[dtype_i, ndim=1] new_ids = np.zeros(ids.shape[0], dtype=np.int64)
+    cdef np.ndarray[dtype_i, ndim=1] rem_ids = np.zeros(ids.shape[0], dtype=np.int64)
+    for counter,ID in enumerate(ids):
+        if ID not in mapvoxels:
+            new_ids[counter] = ID
+        mapvoxels[ID] = np.hstack([mapvoxels[ID][0] + increments[counter], useraddfn(mapvoxels[ID][1:], userdata[counter])])
+        if mapvoxels[ID][0] < 0:
+            del mapvoxels[ID]
+            rem_ids[counter] = ID
+    return mapvoxels, new_ids.astype(np.bool), rem_ids.astype(np.bool)
+
+
+#@cython.boundscheck(False)
+#def shmcopy(manager_list):
+#    elements = []
+#    for el in manager_list:
+#        elements.append(el)
+#    return elements

=== removed file 'mrol_mapping/cython/fast.pyx'
--- mrol_mapping/cython/fast.pyx	2011-07-11 06:52:51 +0000
+++ mrol_mapping/cython/fast.pyx	1970-01-01 00:00:00 +0000
@@ -1,112 +0,0 @@
-#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
-#
-#This program is distributed in the hope that it will be useful,
-#but WITHOUT ANY WARRANTY; without even the implied warranty of
-#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#GNU Lesser General Public License for more details.
-#
-#You should have received a copy of the GNU Lesser General Public License
-#along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-
-cimport cython
-import numpy as np
-cimport numpy as np
-
-#ctypedef np.float64_t dtype_t
-# difference between 32 and 64 bit
-ctypedef np.int64_t dtype_i
-#ctypedef np.int32_t dtype_i
-
-#cdef dict a
-#a = {}
-#a['a'] = 1
-
[e-mail](False)
-def intersection(voxelmap, np.ndarray[dtype_i, ndim=1] voxels):
-    cdef Py_ssize_t i, overlap = 0
-    for i in range(voxels.shape[0]):
-        if voxels[i] in voxelmap:
-            overlap += 1
-    return overlap
-
[e-mail](False)
-#@cython.locals(a=cython.double, b=cython.double,
-               #N=cython.Py_ssize_t, dx=cython.double,
-               #s=cython.double, i=cython.Py_ssize_t)
-#@cython.locals(a=cython.double, b=cython.double,
-def nested_intersection(dict D, np.ndarray[dtype_i, ndim=2] V):
-    cdef Py_ssize_t i, a, b, c, overlap = 0
-    for a, b, c in V:
-        if a in D and b in D[a] and c in D[a][b]:
-            overlap += 1
-    return overlap
-    
-#@cython.boundscheck(False)
-#def intersection(voxelmap, np.ndarray[dtype_i, ndim=2] voxels):
-#    cdef Py_ssize_t i, overlap = 0
-#    for i in range(voxels.shape[0]):
-#        if (voxels[i, 0], voxels[i, 1], voxels[i, 2]) in voxelmap:
-#            overlap += 1
-#    return overlap
-
[e-mail](False)
-def fillray(dtype_i numcells, np.ndarray[np.float64_t, ndim=1] dir_unit, np.float64_t res, np.ndarray[np.float64_t, ndim=2] ray):
-    for i in range(numcells):
-        ray[i] = dir_unit*i*res
-    return ray
-    
[e-mail](False)
-def increment(dict mapvoxels, np.ndarray[dtype_i, ndim=2] voxels, dtype_i amount=1, userdata=None):
-    # TODO ensure this remains consistend with the decrement function
-    # TODO fails when there is only one voxel
-    # TODO add ability to increment according to a vector of occupancies same 
-    # length as the array of voxels ie amount should also be allowed to be a 
-    # vector of occupancies
-    if amount < 0:
-        print "Warning: negative increment amount. Consider using decrement() instead."
-    cdef np.ndarray[dtype_i, ndim=1] newvoxels = np.zeros(voxels.shape[0], dtype=np.int64)
-    cdef dtype_i v0, v1, v2, occupancy
-    cdef tuple v
-    for i in range(voxels.shape[0]):
-        occupancy = 0
-        v0 = voxels[i,0]
-        v1 = voxels[i,1]
-        v2 = voxels[i,2]
-        v = (v0, v1, v2) # ensure v is a tuple
-        if mapvoxels.has_key(v):
-            newvoxels[i] = 0
-            occupancy = mapvoxels[v] + amount
-        else:
-            newvoxels[i] = 1
-            occupancy = amount
-        if occupancy > 0:
-            if userdata == None:
-                mapvoxels[v] = occupancy
-            else:
-                mapvoxels[v] = (occupancy, userdata)
-        else:
-            del mapvoxels[v]
-    return newvoxels.astype(np.bool)
-    
-
[e-mail](False)
-def user_increment(mapvoxels, np.ndarray[np.int_t, ndim=1] ids, np.ndarray[np.float64_t, ndim=1] increments, userdata, useraddfn):
-    cdef np.ndarray[dtype_i, ndim=1] new_ids = np.zeros(ids.shape[0], dtype=np.int64)
-    cdef np.ndarray[dtype_i, ndim=1] rem_ids = np.zeros(ids.shape[0], dtype=np.int64)
-    for counter,ID in enumerate(ids):
-        if ID not in mapvoxels:
-            new_ids[counter] = ID
-        mapvoxels[ID] = np.hstack([mapvoxels[ID][0] + increments[counter], useraddfn(mapvoxels[ID][1:], userdata[counter])])
-        if mapvoxels[ID][0] < 0:
-            del mapvoxels[ID]
-            rem_ids[counter] = ID
-    return mapvoxels, new_ids.astype(np.bool), rem_ids.astype(np.bool)
-
-
-#@cython.boundscheck(False)
-#def shmcopy(manager_list):
-#    elements = []
-#    for el in manager_list:
-#        elements.append(el)
-#    return elements

=== added file 'mrol_mapping/depth_image.py'
--- mrol_mapping/depth_image.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/depth_image.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,141 @@
+'''Author: Julian Ryde'''
+from __future__ import division
+import numpy as np
+import pylab
+import sys
+import mrol_mapping.pointcloud as pointcloud
+
+# TODO generalise this so we can put it into MROL_mapping lib
+def load_image(fname):
+    ''' Load a depth image, generated from Blender '''
+    di = pylab.imread(fname)
+    # convert to ranges
+    # blender map scale factor = 0.2
+    blender_map = 0.2
+    di /= blender_map
+    return di
+
+def image_to_points(di, rgb=None, max_range=5., fov=np.radians(75),
+                    timestamp=0):
+    ''' Convert a depth image to point cloud'''
+    # at edge tan fov/2 = 320 / a where a is the adjacent pixel length
+    a = 0.5 * di.shape[1] / np.tan(fov/2)
+    ci = np.arange(0, di.shape[1])
+    ri = np.arange(0, di.shape[0])
+    cols = ci - di.shape[1]/2 + 0.5
+    rows = ri - di.shape[0]/2 + 0.5
+    rows.shape = -1, 1
+
+    f = di / a
+    # scale factors between image plane and real space position of 
+    # point, then by similar shapes, one is just a scale factor enlargement of the other
+    # Blender z buffer returns not the range to the point but the distance from the camera plane, namely the z distance.
+
+    # TODO could use a masked array for performance
+
+    # camera looking along the x-axis
+    if (len(di.shape) == 3):
+        # adjust for broadcasting
+        cols = np.reshape(cols, (-1, 1))
+        rows = np.reshape(rows, (-1, 1, 1))
+
+    Y = -f * cols
+    Z = -f * rows
+    X = f * a 
+
+    #inds = np.logical_and(di < di.max(), di > 0)
+    inds = np.logical_and(di < max_range, di > 0)
+    
+    xyzs = np.vstack((X[inds], Y[inds], Z[inds])).T
+    assert(len(xyzs))
+    pc = pointcloud.PointCloud(xyzs, timestamp=timestamp)
+    if rgb is not None:
+        rgbfiltered = rgb[inds]
+        pc.userdata = rgbfiltered
+    return pc
+    
+def depth_discont_simple(di, tol=0.2):
+    dx = np.diff(di, axis=0)
+    #dx = np.vstack([np.zeros(di.shape[1]),dx])
+    #dx = np.vstack([np.zeros(di.shape[1]),dx[:-1,:],np.zeros(di.shape[1])])
+    dx = np.vstack([dx,np.zeros(di.shape[1])])
+    dx = np.abs(dx)
+    dx[dx < tol] = 0
+    dy = np.diff(di, axis=1)
+    #dy = np.hstack([np.atleast_2d(np.zeros(di.shape[0])).T,dy])
+    #dy = np.hstack([np.atleast_2d(np.zeros(di.shape[0])).T,dy[:,:-1],np.atleast_2d(np.zeros(di.shape[0])).T])
+    dy = np.hstack([dy,np.atleast_2d(np.zeros(di.shape[0])).T])
+    dy = np.abs(dy)
+    dy[dy < tol] = 0
+    diff_im = np.sqrt(dx**2 + dy**2)
+    di_diff = np.zeros(di.shape)
+    di_diff[diff_im > 0] = di[diff_im > 0]
+    import pylab
+    pylab.ion()
+    pylab.imshow(di_diff)
+    import scipy.ndimage as ndimage
+    #di_diff = ndimage.minimum_filter(di_diff, size=(1,1))
+    #for i in range(di_diff.shape[0]):
+    #    for j in range(di_diff.shape[1]-1):
+    #        p1 = di_diff[i,j]
+    #        p2 = di_diff[i,j+1]
+    #        if p1 == 0 or p2 == 0:
+    #           continue
+    #        if p1 > p2:
+    #            p1 = 0
+    #        elif p2 > p1:
+    #            p2 = 0
+    #        di_diff[i,j] = p1
+    #        di_diff[i,j+1] = p2
+    return di_diff
+    
+def depth_discont_sobel(di, tol=0.5):
+    import scipy.ndimage as ndimage
+    dx = ndimage.convolve(di,[[-1,0,1],[-2,0,2],[-1,0,1]])
+    dy = ndimage.convolve(di,[[-1,-2,-1],[0,0,0],[1,2,1]])
+    diff_im = np.sqrt(dx**2 + dy**2)
+    diff_im[diff_im < tol] = 0
+    di_diff = np.zeros(di.shape)
+    di_diff[diff_im > 0] = di[diff_im > 0]
+    #import pylab
+    #pylab.ion()
+    di_diff = ndimage.minimum_filter(di_diff, size=(2,2))
+    #for i in range(di_diff.shape[0]):
+    #    for j in range(di_diff.shape[1]-1):
+    #        p1 = di_diff[i,j]
+    #        p2 = di_diff[i,j+1]
+    #        if p1 == 0 or p2 == 0:
+    #           continue
+    #        if p1 > p2:
+    #            p1 = 0
+    #        elif p2 > p1:
+    #            p2 = 0
+    #        di_diff[i,j] = p1
+    #        di_diff[i,j+1] = p2
+    #pylab.imshow(di_diff)
+    return di_diff
+
+def depth_filt_min_non_zero(p1,p2):
+    if p1 == 0 or p2 == 0:
+        return
+    if p1 > p2:
+        p1 = 0
+        return
+    if p2 > p1:
+        p2 = 0
+        return
+    return
+    
+def depth_disconts2(di, tol=0.2):
+    import scipy.ndimage as ndimage
+    ndimage.gaussian_filter(di,sigma=(5,5))
+    ndimage.gaussian_derivative()
+    ndimage.median_filter()
+    return di_diff
+
+if __name__ == '__main__':
+    fname = sys.argv[1]
+    xyzs = image_to_points(fname)
+    import mrol_mapping.visualiser.dispxyz as dispxyz
+    dispxyz.showpts(xyzs)
+    print 'Escape to close window'

=== added file 'mrol_mapping/log.py'
--- mrol_mapping/log.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/log.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,11 @@
+import logging
+def getLogger(name):
+    logger = logging.getLogger(name)
+    # create console handler with a higher log level
+    ch = logging.StreamHandler()
+    ch.setLevel(logging.DEBUG)
+    # create formatter and add it to the handlers
+    formatter = logging.Formatter('%(name)s - %(message)s')
+    ch.setFormatter(formatter)
+    logger.addHandler(ch)
+    return logger

=== added file 'mrol_mapping/mapper.py'
--- mrol_mapping/mapper.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/mapper.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,438 @@
+#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
+#
+#This program is distributed in the hope that it will be useful,
+#but WITHOUT ANY WARRANTY; without even the implied warranty of
+#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#GNU Lesser General Public License for more details.
+#
+#You should have received a copy of the GNU Lesser General Public License
+#along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+from __future__ import division
+import numpy as np
+
+import pointcloud
+import occupiedlist
+import mrol
+import poseutil
+import util
+import cPickle as pickle
+import quantizer
+import time
+import log
+import logging
+
+logger = log.getLogger('mapper')
+logger.setLevel(logging.INFO)
+USE_BACKTRACING = True
+
+_map_resolution_levels = 4 # number of different levels of resolution
+
+def loadmap(fname):
+    return pickle.load(open(fname))
+
+class MapBuilder(object):
+    def __init__(self, resolution,
+                 visualise=False,
+                 levels=_map_resolution_levels,
+                 odometry_map=False,
+                 rotate=False):
+        self.vmap = VoxelMap(resolution, levels=levels,
+                             rotate=rotate)
+        self.odometry_map  = odometry_map
+
+        self.bestpose = poseutil.Pose3D()
+        self.scancount = 0
+        if visualise:
+            self.vmap.display()
+
+    def add_pointcloud(self, pc):
+        if self.scancount == 0:
+            self.scancount = 1
+            self.vmap.add_scan(pc, pc.pose)
+            pc.pose = (poseutil.Pose3D() if pc.pose is None else pc.pose)
+            return self.vmap.get_voxels(level=-1), pc.pose
+
+        # reduce number of scan points to one per voxel
+        # sample = pc.volumetricsample(self.vmap.resolution)
+
+        logger.info('--------- Scan %d ---------' % self.scancount)
+        self.scancount += 1
+
+        if pc.pose is None: 
+            guessPose = self.bestpose 
+        else:
+            guessPose = pc.pose
+
+        start = time.time()
+        if not self.odometry_map:
+            self.bestpose, maxcost = self.vmap.align_points(pc, guessPose)
+
+        alignment_time = time.time() - start
+
+        start = time.time()
+        if self.odometry_map: # create map without aligning
+            self.vmap.add_scan(pc, guessPose) 
+        else:
+            self.vmap.add_scan(pc, self.bestpose)
+
+        mapupdate_time = time.time() - start
+        logger.info('Map voxel count:{0}'.format( len(self.vmap)))
+
+        logger.info('Aligned pose:{0}'.format(self.bestpose))
+        logger.info('Cost value: {0}'.format(maxcost))
+
+        logger.info('Alignment time(s):  %.2f' % alignment_time)
+        logger.info('Map update time(s): %.2f' % mapupdate_time)
+        return self.vmap.get_voxels(level=-1), self.bestpose
+
+def buildmap(pointclouds, resolution, visualise=True, odometry_map=False,
+             rotate=False,
+             bestpose_queue=None,
+             voxel_queue=None):
+    '''Takes an iterable of point clouds which can have guess poses and produce 
+    a map.  If no guess poses uses previous pose as an estimate
+
+    bestpose_queue: A util.Queue object through which a mapper thread can
+    optionally communicate back the bestpose got so far. This might be used to
+    optimize the pc.pose i.e. the initial guesspose
+    '''
+    vmap = VoxelMap(resolution, levels=_map_resolution_levels, rotate=rotate)
+    pc = pointclouds.next() 
+    vmap.add_scan(pc, pc.pose)
+
+    logger.info('--------- Scan 0 ---------')
+    logger.info('Map voxel count:{0}'.format(len(vmap)))
+
+    if visualise:
+        vmap.display()
+
+    bestpose = poseutil.Pose3D() 
+    # in case this needs to be used as the first guess pose
+
+    i = 0
+    maxcost = None
+    while True:
+        try:
+            pc = pointclouds.next()
+        except StopIteration:
+            break
+        i += 1
+
+        # reduce number of scan points to one per voxel
+        sample = pc.volumetricsample(resolution)
+
+        logger.info('--------- Scan %d ---------' % i)
+
+        if pc.pose is None: 
+            guessPose = bestpose 
+        else:
+            guessPose = pc.pose
+
+        start = time.time()
+        if not odometry_map:
+            bestpose, maxcost = vmap.align_points(sample, guessPose)
+            if bestpose_queue:
+                bestpose_queue.put_force(bestpose)
+
+        alignment_time = time.time() - start
+
+        start = time.time()
+        if odometry_map: # create map without aligning
+            vmap.add_scan(pc, guessPose) 
+        else:
+            vmap.add_scan(pc, bestpose)
+
+        if voxel_queue:
+            (vdata, vres) = vmap.get_voxels(level=-1)
+            voxel_queue.put_force((vdata, vres))
+
+        mapupdate_time = time.time() - start
+        logger.info('Map voxel count:{0}'.format( len(vmap)))
+
+        logger.info('Aligned pose:{0}'.format(bestpose))
+        logger.info('Cost value: {0}'.format(maxcost))
+
+        #logger.info('Execution times (s):  %.2f\t%.2f' % (alignment_time, mapupdate_time))
+        logger.info('Alignment time(s):  %.2f' % alignment_time)
+        logger.info('Map update time(s): %.2f' % mapupdate_time)
+    return vmap
+
+def _build_Kinect_sensor_model(resolution, minR=0.5, maxR=5):
+    '''builds an occupiedlist sensor model aligned with the x-axis at the 
+    origin for free space determination'''
+    fov = np.radians(75)
+    #hres = 640
+    #vres = 480
+    hres = 320
+    vres = 240
+    dtheta = fov / hres
+    A = 0.5 * hres / np.tan(0.5 * fov)
+    # perpendicular distance of camera image plane in pixel space
+    cols = np.arange(hres) - hres * 0.5
+    rows = np.arange(vres) - vres * 0.5
+    rows.shape = -1, 1
+    # normalise by A so you can step in x-axis
+    rows /= A
+    cols /= A
+    Y = np.tile(cols, (vres, 1)).ravel()
+    Z = np.tile(rows, (1, hres)).ravel()
+
+    sensor_model_ol = occupiedlist.OccupiedList(resolution, maxvoxels=2e6)
+    # this way voxels that have multiple rays passing through them are not 
+    # tested multiple times.
+
+    xyzs = np.empty((vres * hres, 3))
+    for X in np.arange(minR, maxR, resolution):
+        # TODO this is an approximation calculate the angles
+        xyzs[:, 0] = X
+        xyzs[:, 1] = Y * X
+        xyzs[:, 2] = Z * X
+        sensor_model_ol.add_points(xyzs)
+        print X
+    return sensor_model_ol
+
+class VoxelMap:
+    '''Mobile robot mapper capable of 2D and 3D maps.  Need to initialise with 
+    a initial pose and scan so that future scans can be aligned, poses are all 
+    poseutil objects.  Recommended way is to take a 3D scan 
+    whilst the robot is stationary to seed the map and then go from there,
+    matching 2D or 3D scans to the map for localisation within the map,
+    or incrementally growing the map.'''
+
+    # Special functions #############################################
+    # optional also include an estimate of the pose error
+    def __init__(self, resolution, levels=1, rotate=False):
+        # lattice_type='cubic', #alignment_method='OVL','Pt2Pt_ICP','Pt2Pl_ICP'
+        self.mrol = mrol.MROL(resolution, levels)
+        self.verbosity = 0
+        self.trajectory = [] 
+        # List of poses at which observations have been taken
+
+        # The verbosity level. Higher verbosity gives more print statements.
+        #self.set_feature_range(100) 
+        self.lattice = 'cubic' # not used anywhere yet because cubic is default
+        # for assistance in discretisation of angular increments in the 
+        # gauss_siedel optimisation
+        self._sensor_pts = None # the sensor model as voxel points
+        self._rv = None # the robot visualiser
+        self._rotate = rotate # if visualising, autorotate
+
+    def __len__(self):
+        ''' return the number of voxels at the finest resolution '''
+        return len(self.mrol.mapvoxels[-1])
+        
+    #def __str__(self)
+    #    ''' return key statistics of the map as a string: e.g. mapped volume, 
+    #    extents, number of voxels, resolution levels, resolutions.
+
+    # Getter functions #############################################
+    def get_resolution(self):
+        return self.mrol.getfinest().resolution
+
+    def get_voxels(self, level=-1):
+        '''Returns the (almost) raw voxel information together
+        with the resolution of the voxelisation.'''
+        voxels = self.mrol.getvoxels(level=level)
+        resolution = self.mrol.getresolution(level=level)
+        return voxels, resolution
+            
+    def get_points(self, level=-1):
+        '''Returns the map as a point cloud of the voxel centers. If the
+        map has been setup with multiple resolutions, return the voxels at the 
+        desired resolution. Defaults to the finest resolution.'''
+        voxel_centers = self.mrol.getpoints(level=level)
+        pc = pointcloud.PointCloud(voxel_centers)
+        return pc
+        
+
+    def get_overlap(self, pose, xyzs):
+        '''Return the overlap between a pointcloud at a given pose and the map.'''
+        ol = self.mrol.getfinest()
+        overlap = ol.calccollisions(pose, xyzs)
+        return overlap
+        
+    # other functions #############################################
+    
+    def save(self, fname):
+        outfile = open(fname, 'w')
+        self._rv = None # remove visualiser
+        pickle.dump(self, outfile, protocol=2)
+
+    def display(self,npts=1e6,changes=False):
+        '''Shows a representation of the map in a gui'''
+        from visualiser.robotvisualiser import Visualiser
+        if self._rv is None:
+            self._rv_changes = changes
+            self._rv = Visualiser(npts=npts, Rotate=self._rotate)
+            self._rv.set_orthographic(Bool=True)
+
+            # TODO visualiser should take a point cloud object
+            self._rv.addmappts(self.get_points().points)
+
+            # add the trajectory points
+            for traj in self.trajectory:
+                self._rv.addtrajectorypoint(traj.getPosition())
+        #else:
+            #self._disp.se
+
+    def add_scan(self, pc, pose, timestamp=None):
+        '''Adds a new scan to the existing map.
+
+        Adds the occupied voxels and removes the freespace.
+        '''
+        userdata = pc.userdata
+        xyzs = pc.points
+        if USE_BACKTRACING:
+            self.clear_freespace(xyzs, pose)
+        self.add_points(xyzs, pose, timestamp, userdata)
+
+    def clear_freespace(self, xyzs, pose):
+        '''Removes the free space voxels from the occupancy list.
+
+        Parameters:
+            xyzs: The occupied PointCloud, with the sensor as origin.
+            pose: The pose of `xyzs` PointCloud with respect to global map.
+        '''
+        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
+        sensor_loc = poseutil.transformPoints(np.array([[0, 0, 0]]), pose)
+        self.mrol.clear_freespace(xyzs, sensor_loc[0])
+
+    def add_points(self, xyzs, pose, timestamp=None, userdata=None):
+        '''Adds the pointcloud xyzs to the map, transforming them by the given 
+        pose if required. Returns the overlap of the new points against the 
+        existing map.'''
+        if isinstance(xyzs, pointcloud.PointCloud):
+            xyzs = xyzs.points
+        assert xyzs.shape[1] == 3, '3 dimensional points'
+        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
+        self.trajectory.append(return_pose)
+        if self._rv is not None: # visualisation
+            finest = self.mrol.getfinest()
+            # determine unique new voxels convert to points and add to the map
+            #ol = occupiedlist.OccupiedList(finest.resolution)
+            ol = finest
+            #ol.add_points(xyzs)
+            #ol = ol - finest
+            vox = ol.getvoxeldata()
+            self._rv.addmappts(vox)
+            #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2]))
+            self._rv.setrobotpose(return_pose.getMatrix())
+            self._rv.addtrajectorypoint(return_pose.getTuple()[0:3])
+            if self._rv_changes:
+                inliers, outliers = self.mrol.getfinest().segment_ovl(None,xyzs)
+                self._rv.setrightpts(inliers)
+                self._rv.setleftpts(outliers)
+        self.mrol.add_points(xyzs, userdata=userdata, timestamp=timestamp)
+        return self.get_overlap(return_pose.getTuple(), xyzs)
+        
+    # TODO Consider a remove_points based on timestamps?
+    def remove_points(self, xyzs, pose):
+        '''Remove points from the map. In practice, the idea is 
+        to decrement the probability of the voxel in the map having been
+        observed.
+        
+        Returns the voxels at the finest resolution that are now empty.
+        '''
+        xyzs = poseutil.transformPoints(xyzs,pose)[0]
+        return self.mrol.removepoints(xyzs)
+
+    #def set_feature_range(self, feature_range):
+    #    ''' Sets the 'feature_range' variable. Used for assistance in
+    #    the discretisation of angular increments in the gauss_siedel
+    #    optimisation '''
+    #    self.feature_range = float(feature_range)
+    #    self.mrol.feature_range = self.feature_range
+    
+    # def set_options? instead of a long list of init options
+    # def get_options? so the user knows the current setup
+
+        
+    #def some_sort_of_map_maintenance_interface 
+    # - raytrace cleanup, time decay, or callable method based on number 
+    # of observations.
+    # - perhaps as a parameter to the init for auto-maintenance?
+    
+    # If we provide an interface to directly add voxels (e.g. from 
+    # pre-made maps), it would be nice to be able to merge voxels of 
+    # different resolutions and lattice types. This is a low priority
+    # feature
+    #def add_voxels(self, voxels, res, lattice_type=self.lattice_type) # perhaps rename add_map? 
+    
+    #def get_voxels(self, res/level, lattice_type=self.lattice_type) # perhaps rename get_map?
+    # return center coordinate in same format as points are added so it looks like a point cloud
+    
+    #def get_voxel_data 
+    # for things like number of observations. Can also be retrieved from 
+    # get_voxels with the raw option?
+    
+    #def get_voxel_primitive(self, res, lattice_type=self.lattice_type) 
+    # return some representation of the voxel shape for visualisation purposes 
+    # (tri/quad-mesh, corner points?)
+
+    #def sample_points(self, method='volumetric') 
+    # to do some point cloud sampling (volumetric, random etc)
+        
+    def align_points(self, xyzs, guess_pose, two_D=False, full_data=False):
+        '''Align the point cloud xyzs with the data already contained in the 
+        map, using the guess_pose as a seed to the optimisation. Returns the 
+        pose that best fits the point cloud data to the map and the value of 
+        the objective function at that pose.
+        
+        If the map is a multi-resolution structure, then the alignment will use 
+        the data at the various resolutions to perform the alignment, which is 
+        more expensive, but more robust. To force alignment at just a single 
+        resolution, specify the level parameter.
+        
+        By default, the alignment is a 6DOF optimization. This can be 
+        constrained to the XY plane by enabling the two_D parameter.'''
+
+        xyzs = xyzs.points[:,:3]
+        bestpose, cost = self.mrol.optimise_alignment(xyzs, guess_pose, full_data=full_data)
+        return bestpose, cost
+        
+    #def align_points_to_map(self, xyzs, guess_pose):
+    #    bestpose, cost = optimization.gauss_siedel(self._performance_objectiveFast, guess_pose, args=(xyzs, 1.0), quiet=True)
+    #    xyzs_xformed = poseutil.transformPoints(xyzs, bestpose)
+    #    return bestpose, cost, xyzs_xformed
+    
+    def generate_freespace(self, resolution, pose=None, minR=0.5, maxR=5):
+        '''Generates an occuplied list consisting of the observed free space at 
+        the specified resolution.  This is useful for detecting object removal 
+        from a map.'''
+        assert resolution >= self.get_resolution()
+        if self._sensor_pts is None:
+            sensor_model_ol = _build_Kinect_sensor_model(resolution, minR=minR, maxR=maxR) # for simulated data
+            self._sensor_pts = sensor_model_ol.getpoints()
+            logger.info("Sensor model points:", len(self._sensor_pts))
+            #vis = visualiser.robotvisualiser.Visualiser(npts=1e7)
+            #vis.addmappts(self._sensor_pts)
+        
+        base_ol = self.mrol.getfinest()
+        freespace_ol = occupiedlist.OccupiedList(resolution, maxvoxels=5e6)
+        # loop through the trajectory of poses
+        # ray trace from each pose and add voxels to the free space occupied voxel list
+        if pose == None:
+            for pose in self.trajectory:
+                logger.info("Pose {0}".format(pose))
+                inliers, outliers = base_ol.segment_ovl(pose, self._sensor_pts) 
+                # TODO some means of not adding points that are further away than the intersection (doesn't work very well without this!)
+                # Julian suggests a masking operation whilst stepping through the layers of the model in X
+                freespace_ol.add_points(outliers)
+                freespace_ol.removepoints(inliers)
+        else:
+            inliers, outliers = base_ol.segment_ovl(pose, self._sensor_pts) 
+            # TODO some means of not adding points that are further away than the intersection
+            freespace_ol.add_points(outliers)
+            freespace_ol.removepoints(inliers)
+        return freespace_ol
+        
+    def _trajectory_objective(self, guess_pose, pts, foo):           
+        return -occupiedlist.calccollisions(guess_pose, self._traj_alignment_voxel_set, self.res, pts)
+
+    # TODO add support for not specifying centrepose or spreadpose, looks
+    # through entire map
+    def globally_localise(self, points, centrepose, spreadpose):
+        '''Localises the point cloud in this map without requiring a good
+        initial guess pose.  Returns the bestpose along with measure of overlap.'''
+        return self.mrol.match(points.points, centrepose, spreadpose)

=== removed file 'mrol_mapping/mapper.py'
--- mrol_mapping/mapper.py	2012-03-21 18:57:49 +0000
+++ mrol_mapping/mapper.py	1970-01-01 00:00:00 +0000
@@ -1,438 +0,0 @@
-#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
-#
-#This program is distributed in the hope that it will be useful,
-#but WITHOUT ANY WARRANTY; without even the implied warranty of
-#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#GNU Lesser General Public License for more details.
-#
-#You should have received a copy of the GNU Lesser General Public License
-#along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-from __future__ import division
-import numpy as np
-
-import pointcloud
-import occupiedlist
-import mrol
-import poseutil
-import util
-import cPickle as pickle
-import quantizer
-import time
-
-debug = True
-_map_resolution_levels = 4 # number of different levels of resolution
-
-def loadmap(fname):
-    return pickle.load(open(fname))
-
-def buildmap(pointclouds, resolution, visualise=True, odometry_map=False, rotate=False):
-    '''Takes an iterable of point clouds which can have guess poses and produce 
-    a map.  If no guess poses uses previous pose as an estimate'''
-
-    vmap = VoxelMap(resolution, levels=_map_resolution_levels, rotate=rotate)
-    pc = pointclouds.next() 
-    vmap.add_points(pc, pc.pose)
-    if debug:
-        print '--------- Scan 0 ---------'
-        print 'Map voxel count:', len(vmap)
-
-    if visualise:
-        vmap.display()
-
-    bestpose = poseutil.Pose3D() 
-    # in case this needs to be used as the first guess pose
-
-    i = 0
-    maxcost = None
-    while True:
-        try:
-            pc = pointclouds.next()
-        except StopIteration:
-            break
-        i += 1
-
-        # reduce number of scan points to one per voxel
-        sample = pc.volumetricsample(resolution)
-
-        if debug:
-            print '--------- Scan %d ---------' % i
-
-        if pc.pose is None: 
-            guessPose = bestpose 
-        else:
-            guessPose = pc.pose
-
-        start = time.time()
-        if not odometry_map:
-            bestpose, maxcost = vmap.align_points(sample, guessPose)
-        alignment_time = time.time() - start
-
-        start = time.time()
-        if odometry_map: # create map without aligning
-            vmap.add_points(pc, guessPose) 
-        else:
-            vmap.add_points(pc, bestpose)
-        mapupdate_time = time.time() - start
-        print 'Map voxel count:', len(vmap)
-
-        if debug: 
-            print 'Aligned pose:', bestpose
-            print 'Cost value: ', maxcost
-
-        if debug:
-            #print 'Execution times (s):  %.2f\t%.2f' % (alignment_time, mapupdate_time)
-            print 'Alignment time(s):  %.2f' % alignment_time
-            print 'Map update time(s): %.2f' % mapupdate_time
-    return vmap
-
-def _build_Kinect_sensor_model(resolution, minR=0.5, maxR=5):
-    '''builds an occupiedlist sensor model aligned with the x-axis at the 
-    origin for free space determination'''
-    fov = np.radians(75)
-    #hres = 640
-    #vres = 480
-    hres = 320
-    vres = 240
-    dtheta = fov / hres
-    A = 0.5 * hres / np.tan(0.5 * fov)
-    # perpendicular distance of camera image plane in pixel space
-    cols = np.arange(hres) - hres * 0.5
-    rows = np.arange(vres) - vres * 0.5
-    rows.shape = -1, 1
-    # normalise by A so you can step in x-axis
-    rows /= A
-    cols /= A
-    Y = np.tile(cols, (vres, 1)).ravel()
-    Z = np.tile(rows, (1, hres)).ravel()
-
-    sensor_model_ol = occupiedlist.OccupiedList(resolution, maxvoxels=2e6)
-    # this way voxels that have multiple rays passing through them are not 
-    # tested multiple times.
-
-    xyzs = np.empty((vres * hres, 3))
-    for X in np.arange(minR, maxR, resolution):
-        # TODO this is an approximation calculate the angles
-        xyzs[:, 0] = X
-        xyzs[:, 1] = Y * X
-        xyzs[:, 2] = Z * X
-        sensor_model_ol.add_points(xyzs)
-        print X
-    return sensor_model_ol
-
-class VoxelMap:
-    '''Mobile robot mapper capable of 2D and 3D maps.  Need to initialise with 
-    a initial pose and scan so that future scans can be aligned, poses are all 
-    poseutil objects.  Recommended way is to take a 3D scan 
-    whilst the robot is stationary to seed the map and then go from there,
-    matching 2D or 3D scans to the map for localisation within the map,
-    or incrementally growing the map.'''
-
-    # Special functions #############################################
-    # optional also include an estimate of the pose error
-    def __init__(self, resolution, levels=1, rotate=False):
-        # lattice_type='cubic', #alignment_method='OVL','Pt2Pt_ICP','Pt2Pl_ICP'
-        self.mrol = mrol.MROL(resolution, levels)
-        self.verbosity = 0
-        self.trajectory = [] 
-        # List of poses at which observations have been taken
-
-        # The verbosity level. Higher verbosity gives more print statements.
-        #self.set_feature_range(100) 
-        self.lattice = 'cubic' # not used anywhere yet because cubic is default
-        # for assistance in discretisation of angular increments in the 
-        # gauss_siedel optimisation
-        self._sensor_pts = None # the sensor model as voxel points
-        self._rv = None # the robot visualiser
-        self._rotate = rotate # if visualising, autorotate
-
-    def __len__(self):
-        ''' return the number of voxels at the finest resolution '''
-        return len(self.mrol.mapvoxels[-1])
-        
-    #def __str__(self)
-    #    ''' return key statistics of the map as a string: e.g. mapped volume, 
-    #    extents, number of voxels, resolution levels, resolutions.
-
-    # Getter functions #############################################
-    def get_resolution(self):
-        return self.mrol.getfinest().resolution
-
-    def get_voxels(self, level=-1):
-        '''Returns the (almost) raw voxel information together
-        with the resolution of the voxelisation.'''
-        voxels = self.mrol.getvoxels(level=level)
-        resolution = self.mrol.getresolution(level=level)
-        return voxels[0], voxels[1], resolution
-            
-    def get_points(self, level=-1):
-        '''Returns the map as a point cloud of the voxel centers. If the
-        map has been setup with multiple resolutions, return the voxels at the 
-        desired resolution. Defaults to the finest resolution.'''
-        voxel_centers = self.mrol.getpoints(level=level)
-        pc = pointcloud.PointCloud(voxel_centers)
-        return pc
-        
-
-    def get_overlap(self, pose, xyzs):
-        '''Return the overlap between a pointcloud at a given pose and the map.'''
-        ol = self.mrol.getfinest()
-        overlap = ol.calccollisions(pose, xyzs)
-        return overlap
-        
-    # other functions #############################################
-    
-    def save(self, fname):
-        outfile = open(fname, 'w')
-        self._rv = None # remove visualiser
-        pickle.dump(self, outfile, protocol=2)
-
-    def display(self,npts=1e6,changes=False):
-        '''Shows a representation of the map in a gui'''
-        import visualiser.robotvisualiser
-        if self._rv is None:
-            self._rv_changes = changes
-            self._rv = visualiser.robotvisualiser.Visualiser(npts=npts, Rotate=self._rotate)
-            self._rv.set_orthographic(Bool=True)
-            #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2]))
-            #self._rv.setautominmax()
-
-            #if len(inliers) > 0:
-            #    vis.setrightpts(inliers)
-            #    vis.setleftpts(outliers)
-            #vis.setspinpts(FreeMapper.get_points())
-
-            # TODO visualiser should take a point cloud object
-            self._rv.addmappts(self.get_points().points)
-
-            # add the trajectory points
-            for traj in self.trajectory:
-                self._rv.addtrajectorypoint(traj.getPosition())
-        #else:
-            #self._disp.se
-
-    def add_points(self, xyzs, pose, timestamp=None, userdata=None):
-        '''Adds the pointcloud xyzs to the map, transforming them by the given 
-        pose if required. Returns the overlap of the new points against the 
-        existing map.'''
-        xyzs = xyzs.points
-        if xyzs.shape[1] > 3: # handling for additional information that is part of the pointcloud object (e.g. colour)
-            if userdata == None:
-                userdata = xyzs[:,3:]
-            else:
-                userdata = np.hstack([xyzs[:,3:],userdata])
-            xyzs = xyzs[:,:3]
-        xyzs, return_pose = poseutil.transformPoints(xyzs, pose)
-        self.trajectory.append(return_pose)
-        if self._rv is not None: # visualisation
-            finest = self.mrol.getfinest()
-            # determine unique new voxels convert to points and add to the map
-            ol = occupiedlist.OccupiedList(finest.resolution)
-            ol.add_points(xyzs)
-            ol = ol - finest
-            self._rv.addmappts(ol.getpoints())
-            #self._rv.setminmax(np.min(self.get_points().points[:,2]),np.max(self.get_points().points[:,2]))
-            self._rv.setrobotpose(return_pose.getMatrix())
-            self._rv.addtrajectorypoint(return_pose.getTuple()[0:3])
-            if self._rv_changes:
-                inliers, outliers = self.mrol.getfinest().segment_ovl(None,xyzs)
-                self._rv.setrightpts(inliers)
-                self._rv.setleftpts(outliers)
-        self.mrol.add_points(xyzs, userdata=userdata, timestamp=timestamp)
-        return self.get_overlap(return_pose.getTuple(), xyzs)
-        
-    # TODO Consider a remove_points based on timestamps?
-    def remove_points(self, xyzs, pose):
-        '''Remove points from the map. In practice, the idea is 
-        to decrement the probability of the voxel in the map having been
-        observed.
-        
-        Returns the voxels at the finest resolution that are now empty.
-        '''
-        xyzs = poseutil.transformPoints(xyzs,pose)[0]
-        return self.mrol.removepoints(xyzs)
-
-    #def set_feature_range(self, feature_range):
-    #    ''' Sets the 'feature_range' variable. Used for assistance in
-    #    the discretisation of angular increments in the gauss_siedel
-    #    optimisation '''
-    #    self.feature_range = float(feature_range)
-    #    self.mrol.feature_range = self.feature_range
-    
-    # def set_options? instead of a long list of init options
-    # def get_options? so the user knows the current setup
-
-        
-    #def some_sort_of_map_maintenance_interface 
-    # - raytrace cleanup, time decay, or callable method based on number 
-    # of observations.
-    # - perhaps as a parameter to the init for auto-maintenance?
-    
-    # If we provide an interface to directly add voxels (e.g. from 
-    # pre-made maps), it would be nice to be able to merge voxels of 
-    # different resolutions and lattice types. This is a low priority
-    # feature
-    #def add_voxels(self, voxels, res, lattice_type=self.lattice_type) # perhaps rename add_map? 
-    
-    #def get_voxels(self, res/level, lattice_type=self.lattice_type) # perhaps rename get_map?
-    # return center coordinate in same format as points are added so it looks like a point cloud
-    
-    #def get_voxel_data 
-    # for things like number of observations. Can also be retrieved from 
-    # get_voxels with the raw option?
-    
-    #def get_voxel_primitive(self, res, lattice_type=self.lattice_type) 
-    # return some representation of the voxel shape for visualisation purposes 
-    # (tri/quad-mesh, corner points?)
-
-    #def sample_points(self, method='volumetric') 
-    # to do some point cloud sampling (volumetric, random etc)
-        
-    def align_points(self, xyzs, guess_pose, two_D=False, full_data=False):
-        '''Align the point cloud xyzs with the data already contained in the 
-        map, using the guess_pose as a seed to the optimisation. Returns the 
-        pose that best fits the point cloud data to the map and the value of 
-        the objective function at that pose.
-        
-        If the map is a multi-resolution structure, then the alignment will use 
-        the data at the various resolutions to perform the alignment, which is 
-        more expensive, but more robust. To force alignment at just a single 
-        resolution, specify the level parameter.
-        
-        By default, the alignment is a 6DOF optimization. This can be 
-        constrained to the XY plane by enabling the two_D parameter.'''
-
-        xyzs = xyzs.points[:,:3]
-        bestpose, cost = self.mrol.optimise_alignment(xyzs, guess_pose, full_data=full_data)
-        return bestpose, cost
-        
-    #def align_points_to_map(self, xyzs, guess_pose):
-    #    bestpose, cost = optimization.gauss_siedel(self._performance_objectiveFast, guess_pose, args=(xyzs, 1.0), quiet=True)
-    #    xyzs_xformed = poseutil.transformPoints(xyzs, bestpose)
-    #    return bestpose, cost, xyzs_xformed
-    
-    def generate_freespace(self, resolution, pose=None, minR=0.5, maxR=5):
-        '''Generates an occuplied list consisting of the observed free space at 
-        the specified resolution.  This is useful for detecting object removal 
-        from a map.'''
-        assert resolution >= self.get_resolution()
-        if self._sensor_pts is None:
-            sensor_model_ol = _build_Kinect_sensor_model(resolution, minR=minR, maxR=maxR) # for simulated data
-            self._sensor_pts = sensor_model_ol.getpoints()
-            print "Sensor model points:", len(self._sensor_pts)
-            #vis = visualiser.robotvisualiser.Visualiser(npts=1e7)
-            #vis.addmappts(self._sensor_pts)
-        
-        base_ol = self.mrol.getfinest()
-        freespace_ol = occupiedlist.OccupiedList(resolution, maxvoxels=5e6)
-        # loop through the trajectory of poses
-        # ray trace from each pose and add voxels to the free space occupied voxel list
-        if pose == None:
-            for pose in self.trajectory:
-                print pose
-                inliers, outliers = base_ol.segment_ovl(pose, self._sensor_pts) 
-                # TODO some means of not adding points that are further away than the intersection (doesn't work very well without this!)
-                # Julian suggests a masking operation whilst stepping through the layers of the model in X
-                freespace_ol.add_points(outliers)
-                freespace_ol.removepoints(inliers)
-        else:
-            inliers, outliers = base_ol.segment_ovl(pose, self._sensor_pts) 
-            # TODO some means of not adding points that are further away than the intersection
-            freespace_ol.add_points(outliers)
-            freespace_ol.removepoints(inliers)
-        return freespace_ol
-            
-    def generate_freespace2(self,pc=None, pose=None, resolution=None, minR=0.5, maxR=5):
-        '''
-        Uses existing ray-trace method to get 
-        around the current limitations of above implementation.
-        
-        Generates an occuplied list consisting of the observed free space at 
-        the specified resolution.  This is useful for detecting object removal 
-        from a map.
-        '''
-        if resolution == None:
-            res = self.get_resolution()
-        else:
-            res = resolution
-        assert res >= self.get_resolution()
-        free_space_ol = occupiedlist.OccupiedList(res, maxvoxels=2e6)
-        if pc==None and pose==None:
-            # generate from map using pose list
-            if self._sensor_pts is None:
-                sensor_model_ol = _build_Kinect_sensor_model(res, minR=minR, maxR=maxR)
-                self._sensor_pts = pointcloud.PointCloud(sensor_model_ol.getpoints())
-                print "Sensor model points:", len(self._sensor_pts)
-            base_ol = self.mrol.getfinest()
-
-            for pose in self.trajectory:
-                print pose
-                inliers, outliers = base_ol.segment_ovl(pose, self._sensor_pts.points)
-                inlier_pc = pointcloud.PointCloud(inliers)
-                free_pts = self.free_space_ray_trace(inliers,pose.getTuple()[:3],self.get_resolution())
-                free_space_ol.add_points(free_pts.points)
-        else:
-            assert xyzs != None and pose != None, "Both xyzs and pose must be specified"
-            # resampling to align with grid. bad hack, but makes ray_cast easier.
-            voxs = occupiedlist.pointstovoxels(xyzs, res)
-            voxs = quantizer.uniquerows(voxs, 0)
-            X = occupiedlist.voxelstopoints(voxs, res)
-            X_pc = pointcloud.PointCloud(X)
-            free_pts = self.free_space_ray_trace(X_pc,pose.getTuple()[:3],self.get_resolution())
-            free_space_ol.add_points(free_pts.points)
-        return free_space_ol
-
-    def free_space_ray_trace(self,pc,origin,resolution,voxel_offset=0):
-        # This is essentially independant of the VoxelMap; put elsewhere?
-        free_pts = []
-        res = resolution
-        v_off = np.ceil(voxel_offset*(res/self.get_resolution()))
-        free_pts_OL = occupiedlist.OccupiedList(res,maxvoxels=3e6)
-        count = 0
-        for point in pc.points:
-            ray_vox_hits, range2, dir_unit, all_ray_pts = self.ray_trace_pts(origin,point,return_extras=True)
-            all_ray_pts = all_ray_pts.points[:-(v_off+1)]
-            all_ray_vox = occupiedlist.pointstovoxels(all_ray_pts, res)
-
-            all_ray_vox_ol = occupiedlist.OccupiedList(1)
-            all_ray_vox_ol.add_points(all_ray_vox)
-
-            ray_vox_hits_ol = occupiedlist.OccupiedList(1) 
-            ray_vox_hits_ol.add_points(ray_vox_hits.points)
-
-            free_ray_vox_ol = all_ray_vox_ol - ray_vox_hits_ol
-            free_pts_OL.add_points(free_ray_vox_ol.getpoints()*res)
-            count +=1
-            #if np.remainder(count / len(pc) * 100,1) == 0:
-            #    print count / len(pc) * 100 , '%'
-        return pointcloud.PointCloud(free_pts_OL.getpoints())
-            
-            
-
-    def ray_trace(self, pose, length, axis=0):
-        '''perform a ray-trace from the pose and return all map voxels at the
-        base resolution along the ray as a pointcloud object.
-        '''
-        return pointcloud.PointCloud(self.mrol.raytrace(pose, length, axis=axis))
-        
-    def ray_trace_pts(self, point1, point2, return_extras=False):
-        '''
-        perform a ray-trace from point1 to point2 and return all map 
-        voxels at the base resolution along the ray as a pointcloud object.
-        '''
-        if not return_extras:
-            ray_vox_hits = self.mrol.raytracepts(point1, point2, return_extras=False)
-            return pointcloud.PointCloud(ray_vox_hits)
-        else:
-            ray_vox_hits, range2, dir_unit, all_ray_pts = self.mrol.raytracepts(point1, point2, return_extras=True)
-            return pointcloud.PointCloud(ray_vox_hits), range2, dir_unit, pointcloud.PointCloud(all_ray_pts)
-        
-    def _trajectory_objective(self, guess_pose, pts, foo):           
-        return -occupiedlist.calccollisions(guess_pose, self._traj_alignment_voxel_set, self.res, pts)
-
-    # TODO add support for not specifying centrepose or spreadpose, looks
-    # through entire map
-    def globally_localise(self, points, centrepose, spreadpose):
-        '''Localises the point cloud in this map without requiring a good
-        initial guess pose.  Returns the bestpose along with measure of overlap.'''
-        return self.mrol.match(points.points, centrepose, spreadpose)

=== added file 'mrol_mapping/mrol.py'
--- mrol_mapping/mrol.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/mrol.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,354 @@
+'''
+Implementation of multi-resolution occupied voxel lists (MROL) 
+Author: Julian Ryde and  Nick Hillier
+'''
+#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
+#
+#This program is distributed in the hope that it will be useful,
+#but WITHOUT ANY WARRANTY; without even the implied warranty of
+#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#GNU Lesser General Public License for more details.
+#
+#You should have received a copy of the GNU Lesser General Public License
+#along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+from __future__ import division
+
+import time
+import os
+import numpy as np
+
+import occupiedlist
+import optimization
+import util
+import poseutil
+import quantizer
+import logging
+
+logger = logging.getLogger('mrol')
+logger.setLevel(logging.INFO)
+#modalfractions = np.repeat(0.3, 5)
+modalfractions = [0.7, 0.8, 0.9, 0.9, 0.9] 
+topn = 100
+
+# approximate number of sample points selected from the scan to be aligned
+sample_size = 2000
+
+# TODO consider pulling in table class from icra2011 code, table.py
+def printtable(table, width):
+    for row in table:
+        for field in row:
+            print '| ' + str(field).ljust(width)[:width],
+        print
+        
+class MROL:
+    '''
+    Stores voxel indices at multiple resolution occupied voxel lists
+    and provides interfaces to add, remove and manipulate the data.
+    '''
+    def __init__(self, resolution, levels=3):
+        # coarse to fine resolutions
+        self.factor = 2 # multiplier between the resolutions might have to be an integer?
+        resolutions = (pow(self.factor, np.arange(levels))*resolution)[::-1]
+        self.mapvoxels = [] # list of occupied voxel lists
+        self.maptimes = []
+        # TODO: save memory by setting the max voxels at each level 
+        for i, res in enumerate(resolutions):
+            ol = occupiedlist.OccupiedList(res)
+            self.mapvoxels.append(ol)
+            #self.maptimes.append([]) # TODO
+
+        self.keyframe = set()
+        self.feature_range = 8.0
+        # range to a good feature density for angular alignment purposes
+
+    def __repr__(self):
+        return self.__str__()
+
+    def __str__(self):
+        width = 6
+        delim = '\t'
+        resolutions = (str(m.resolution).rjust(width) for m in self.mapvoxels)
+        retstr = 'resolutions:  ' + delim.join(resolutions) + '\nvoxel counts: '
+        voxel_counts = (str(len(m)).rjust(width) for m in self.mapvoxels)
+        retstr += delim.join(voxel_counts)
+        return retstr 
+    
+    def __len__(self):
+        return len(self.mapvoxels)
+
+    def __eq__(self, mrol_map):
+        return self.__dict__ == mrol_map.__dict__
+        
+    def getresolution(self, level=None):
+        if level == None: return self.getfinest().resolution
+        return self.mapvoxels[level].resolution
+
+    def getpoints(self, level=-1):
+        return self.mapvoxels[level].getpoints()
+
+    def getvoxels(self, level=-1):
+        return self.mapvoxels[level].getvoxeldata()
+
+    def getfinest(self):
+        '''
+        Returns the occupied list with the finest/best resolution it has.
+        '''
+        return self.mapvoxels[-1]
+        
+    def clear_freespace(self, xyzs, sensor_loc):
+        for ol in self.mapvoxels:
+            ol.clear_freespace(xyzs, sensor_loc)
+
+    def add_points(self, xyzs, timestamp=None, userdata=None, increment=1):
+        # TODO do something with timestamps for re-journalling
+        # TODO allow userdata to be added as an information field to the voxels 
+        # (e.g. RGB)
+        # TODO work out how to get the userdata out again (perhaps in 
+        # getpoints/getvoxels?)        
+
+        for ol in self.mapvoxels:
+            ol.add_points(xyzs,userdata=userdata)
+
+        ## add to the finest occupiedlist
+        #modified_inds, increments = self.getfinest().add_points(xyzs, return_modified=True, increment=increment)
+        ##modified_inds_finest = modified_inds.copy()
+        ## now propagate these modifications to coarser resolutions
+        #for ol in self.mapvoxels[-2::-1]:
+        #    modified_inds /= self.factor
+        #    # TODO slow but hopefully more likely to be correct way
+        #    ol._update_voxels(modified_inds, increments)
+        ##return modified_inds_finest
+
+    def removepoints(self, xyzs, return_removed=False):
+        return self.add_points(xyzs, increment=-1)
+            
+    # TODO Reconsider keyframe usage after trajectory class is implemented
+    #def setkeyframe(self, xyzs, pose):
+    #    xyzs = poseutil.transformPoints(xyzs,pose)
+    #    voxel_array = occupiedlist.pointstovoxels(xyzs, self.getresolution(), unique=True)
+    #    self.keyframe = set(occupiedlist.hashrows(voxel_array))
+    #    
+    #def getkeyframeset(self):
+    #    return self.keyframe
+        
+    def _sample_points(self, xyzs, sample_size):
+        start_resolution = self.getfinest().resolution
+        sample = util.volumetricsample(xyzs, start_resolution)
+        for i in range(1, 10):
+            sample_res = start_resolution * 2**i
+            sample = util.volumetricsample(sample, sample_res)
+            if len(sample) < sample_size:
+                break
+
+        logger.info('Volumetric sample resolution: {0}'.format(sample_res))
+        return sample
+
+    # Ray trace methods
+    def calcray(self, ray):    
+        ol = self.getfinest()
+        hits = ol.segment_ovl(None, ray)[0] 
+        if len(hits) > 0:
+            vox_hits = occupiedlist.pointstovoxels(hits,ol.resolution,ol.lattice)
+            return quantizer.uniquerows(vox_hits, 0)
+        else:
+            return []
+            
+    def raytrace(self, pose, length, axis=0):
+        '''
+        perform a ray-trace from the pose and return all map voxels at the
+        base resolution along the ray.
+        It traces from any point in the map frame (e.g. the sensor's
+        current position) along a ray oriented as per the pose's angular 
+        components.
+        '''
+        res = self.getresolution()/2.
+        numcells = np.ceil(length/res)+2 # + 1 for 0th point + 1 for end point with poor aligned voxels
+        ray = np.zeros([numcells,3])
+        ray[:,axis] = np.hstack([0,np.cumsum(np.ones(numcells-1))*res])
+        ray = poseutil.transformPoints(ray, pose)[0]
+        return self.calcray(ray)
+        
+    def raytracepts(self, point1, point2, return_extras=False):
+        '''
+        perform a ray-trace from point1 to point2 and return all map 
+        voxels at the base resolution along the ray.
+        '''
+        res = self.getresolution()#/3.
+        point2a = point2 - point1
+        range2 = np.sum(point2a**2)
+        length = np.sqrt(range2)
+        dir_unit = point2a/length
+        numcells = int(np.ceil(length/res)+2) # + 1 for 0th point + 1 for last point
+        ray = np.zeros([numcells,3])
+        ## TODO make this a list comprehension as this loop slows the cast down lots
+        #for i in range(numcells):
+        #    ray[i] = dir_unit*i*res
+        ray_res = length/(numcells-1)
+        ray = fast.fillray(numcells, dir_unit, ray_res, ray)
+        ray = ray + point1
+        if return_extras:
+            return self.calcray(ray), length, dir_unit, ray
+        else:
+            return self.calcray(ray)
+        
+    # Alignment methods
+    def objectivefuncMROL(self, pose, xyzs): # TODO not really used anymore?
+        '''
+        Objective function to be minimised.
+        For a list of xyzs, transforms by pose and calculations the cost
+        at the various resolutions. If level is specified, only 
+        calc at that level.
+        '''
+        levels = len(self.mapvoxels)
+        hits = np.empty(levels)
+        resolutions = np.empty(levels)
+        for i, ol in enumerate(self.mapvoxels):
+            hits[i] = ol.calccollisions(pose, xyzs, query=False)
+            resolutions[i] = ol.resolution
+        # sort hits from coarse to fine
+        new = np.hstack((-np.diff(hits), hits[-1]))
+        #weights = np.ones(levels)
+        weights = resolutions[-1]/resolutions # same as ..., 1/8, 1/4, 1/2, 1
+        #cost = -np.dot(weights, hits)
+        cost = -np.dot(weights, new)
+        return cost
+     
+    def optimise_alignment(self, xyzs, initialpose, full_data=False):
+        dtheta_base = np.radians(0.5)
+        # TODO worried that this is not checking for adjacent voxel occupancy
+
+        # sample scan points appropriately
+        # no point doing multiple look ups on the same voxel and in fact an
+        # unfair weighting
+        # you want a volumetric sample such that the number of sample points is enough
+
+        # cast points to float32 for speed
+        xyzs = np.float32(xyzs)
+        xyzs_sample = self._sample_points(xyzs, sample_size)
+        #xyzs_sample = util.volumetricsample(xyzs, 0.4)
+
+        logger.info('initialpose:{0}'.format(initialpose))
+        logger.info('Number of sample points: %d' % len(xyzs_sample))
+
+        bestpose = initialpose.copy()
+        for level, ol in enumerate(self.mapvoxels):
+            objective_func = ol.cost_func 
+            dtheta = dtheta_base*pow(self.factor,(len(self.mapvoxels)-level-1))
+            dx = ol.resolution
+            # This helps avoid local minima, but limits accuracy to voxel size 
+            # at the finest resolution
+            # objective_func is -occupiedlist.calccollisions(pose, xyzs)
+            bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs_sample,), dx, dtheta, max_iterations=20, verbosity=2)
+            logger.info('resolution: %f' % (ol.resolution))
+            logger.info('bestpose: {0}'.format(bestpose))
+            #bestpose, cost = optimization._cost_min_scipy(objective_func, bestpose, (xyzs_sample,))
+        # run it again at the finest resolution with tighter steps to get 
+        # sub-voxel accuracy.
+        dx = dx*pow(self.factor,-2)
+        dtheta = dtheta_base*pow(self.factor,-2)
+        if not full_data:
+            # Sometimes get better results with the below resampling, but much faster.
+            xyzs = util.offsetvolsample(xyzs, self.getfinest().resolution)
+        bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs,), dx, dtheta, max_iterations=100, verbosity=2)
+        return bestpose, cost
+
+    def match(self, xyzs, centrepose, spreadpose):
+        """Does a global search of the specified pose space.
+        Find the possible locations of the occupied list ol in the current 
+        occupied list.  Takes an occupied list to match to the current one and 
+        two poses."""
+
+        # determines the orientation step size as it relates to occupancy list 
+        # resolution
+        poses = None
+        bestpose = poseutil.Pose3D()
+
+        totaltime = time.time()
+        lasttime = time.time()
+        # TODO think about this number
+        #xyzs = util.getsample(xyzs, 1000)
+
+        levels = len(self.mapvoxels)
+        table = np.zeros((levels+2, 9), dtype=np.dtype('S8'))
+        # header for the table of values of interest
+        table = []
+        table.append('Res modal map    scan   pose  pose  Top     Time  Best'.split())
+        table.append('(m) Frac  voxels points count count overlap Taken pose'.split())
+        table.append('--- ----- ------ ------ ----- ----- ------- ----- ----'.split())
+        for i, mapol in enumerate(self.mapvoxels):
+            tablerow = []
+            res = mapol.resolution
+            deltaposition = res
+            deltaorientation = res/self.feature_range # 8
+            if poses == None:
+                poses = poseutil.uniformposes(centrepose, spreadpose, deltaposition, deltaorientation)
+            else:
+                poses = poseutil.refineposes(poses, deltaposition, deltaorientation)
+            logging.info('Number of poses to test:%d' % len(poses))
+
+            # TODO remove poses outside search space this needs tidying up/thinking about
+            #cparr = np.array(centrepose.get())
+            #sparr = np.array(spreadpose.get())
+            #D = np.array((deltaposition, deltaposition, deltaposition, deltaorientation, deltaorientation, deltaorientation))
+            #maxpose = cparr + sparr + D
+            #minpose = cparr - sparr - D
+            #select = np.logical_and(np.all(poses <= maxpose, 1), np.all(poses >= minpose, 1))
+            ##print 'Culling poses: ', len(poses), sum(select)
+            #poses = poses[select]
+            
+            xyzs_sample = util.volumetricsample(xyzs, res)
+            tablerow.extend((res, modalfractions[i], len(mapol), len(xyzs_sample), len(poses)))
+
+            overlaps = []
+            for pose in poses: #  TIME consuming loop
+                #overlap = np.sum(mapol.calccollisions(pose, xyzs, query=True))
+                overlap = mapol.calccollisions(pose, xyzs_sample, query=False)
+                overlaps.append(overlap)
+                #overlaps.append(-self.objectivefuncMROL(pose, xyzs_sample))
+            overlaps = np.asarray(overlaps)
+
+            ind_max = np.argmax(overlaps)
+            bestpose.set(poses[ind_max])
+            bestoverlap = overlaps[ind_max]
+            candidates = overlaps >= modalfractions[i] * bestoverlap
+            # maybe just take the top n?
+            #if len(overlaps) > topn:
+            #    candidates = overlaps.argsort()[-topn:]
+            #else:
+            #    candidates = range(len(overlaps))
+
+            timetaken = time.time() - lasttime
+            lasttime = time.time()
+            tablerow.extend((len(candidates), bestoverlap, timetaken, bestpose))
+            table.append(tablerow)
+            print
+            printtable(table, width=9)
+            print 'Bestpose: ', bestpose
+            #scansize = ol.voxels[-1].shape[0]
+            poses = poses[candidates]
+
+        print "Total time: ", time.time() - totaltime
+        overlaps = overlaps[candidates]
+        return bestpose, bestoverlap
+        
+    def matchbest(self, xyzs, centrepose, spreadpose):
+        poses, overlaps = self.match(xyzs, centrepose, spreadpose)
+        bestind = np.argmax(overlaps)
+        bestpose = poseutil.Pose3D(poses[bestind, :])
+        cost = overlaps[bestind]
+        return bestpose, cost
+    
+    def matchandadd(self, ol, centrepose, spreadpose):
+        """ matches the given occupiedlist ol by searching around the 
+        centrepose a distance of spreadpose. If the match is successful the 
+        occupied list is transfomred and combined with the current occupied 
+        list. Returns the pose at which the occupied list ol was added."""
+        # TODO add a threshold for deciding whether to merge 
+        # TODO add unit test for this
+        # TODO add a ray trace clear?
+        poses, overlaps = self.match(ol, centrepose, spreadpose)
+        bestpose = poseutil.Pose3D(poses[np.argmax(overlaps), :])
+        self.add_points(bestpose.transformPoints(ol.getpoints()))
+        return bestpose
+

=== removed file 'mrol_mapping/mrol.py'
--- mrol_mapping/mrol.py	2012-03-04 17:10:07 +0000
+++ mrol_mapping/mrol.py	1970-01-01 00:00:00 +0000
@@ -1,352 +0,0 @@
-'''
-Implementation of multi-resolution occupied voxel lists (MROL) 
-Author: Julian Ryde and  Nick Hillier
-'''
-#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
-#
-#This program is distributed in the hope that it will be useful,
-#but WITHOUT ANY WARRANTY; without even the implied warranty of
-#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#GNU Lesser General Public License for more details.
-#
-#You should have received a copy of the GNU Lesser General Public License
-#along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-from __future__ import division
-
-import time
-import os
-import numpy as np
-
-import occupiedlist
-import optimization
-import util
-import poseutil
-import quantizer
-
-#import cython.fast as fast
-
-verbose = True
-#modalfractions = np.repeat(0.3, 5)
-modalfractions = [0.7, 0.8, 0.9, 0.9, 0.9] 
-topn = 100
-
-# approximate number of sample points selected from the scan to be aligned
-sample_size = 2000
-
-# TODO consider pulling in table class from icra2011 code, table.py
-def printtable(table, width):
-    for row in table:
-        for field in row:
-            print '| ' + str(field).ljust(width)[:width],
-        print
-        
-class MROL:
-    '''
-    Stores voxel indices at multiple resolution occupied voxel lists
-    and provides interfaces to add, remove and manipulate the data.
-    '''
-    def __init__(self, resolution, levels=3):
-        # coarse to fine resolutions
-        self.factor = 2 # multiplier between the resolutions might have to be an integer?
-        resolutions = (pow(self.factor, np.arange(levels))*resolution)[::-1]
-        self.mapvoxels = [] # list of occupied voxel lists
-        self.maptimes = []
-        # TODO: save memory by setting the max voxels at each level 
-        for i, res in enumerate(resolutions):
-            ol = occupiedlist.OccupiedList(res)
-            self.mapvoxels.append(ol)
-            #self.maptimes.append([]) # TODO
-
-        self.keyframe = set()
-        self.feature_range = 8.0
-        # range to a good feature density for angular alignment purposes
-
-    def __repr__(self):
-        return self.__str__()
-
-    def __str__(self):
-        width = 6
-        delim = '\t'
-        resolutions = (str(m.resolution).rjust(width) for m in self.mapvoxels)
-        retstr = 'resolutions:  ' + delim.join(resolutions) + '\nvoxel counts: '
-        voxel_counts = (str(len(m)).rjust(width) for m in self.mapvoxels)
-        retstr += delim.join(voxel_counts)
-        return retstr 
-    
-    def __len__(self):
-        return len(self.mapvoxels)
-
-    def __eq__(self, mrol_map):
-        return self.__dict__ == mrol_map.__dict__
-        
-    def getresolution(self, level=None):
-        if level == None: return self.getfinest().resolution
-        return self.mapvoxels[level].resolution
-
-    def getpoints(self, level=-1):
-        return self.mapvoxels[level].getpoints()
-
-    def getvoxels(self, level=-1):
-        return self.mapvoxels[level].getvoxeldata()
-
-    def getfinest(self):
-        '''
-        Returns the occupied list with the finest/best resolution it has.
-        '''
-        return self.mapvoxels[-1]
-        
-    def add_points(self, xyzs, timestamp=None, userdata=None, increment=1):
-        # TODO do something with timestamps for re-journalling
-        # TODO allow userdata to be added as an information field to the voxels 
-        # (e.g. RGB)
-        # TODO work out how to get the userdata out again (perhaps in 
-        # getpoints/getvoxels?)        
-
-        for ol in self.mapvoxels:
-            ol.add_points(xyzs,userdata=userdata)
-
-        ## add to the finest occupiedlist
-        #modified_inds, increments = self.getfinest().add_points(xyzs, return_modified=True, increment=increment)
-        ##modified_inds_finest = modified_inds.copy()
-        ## now propagate these modifications to coarser resolutions
-        #for ol in self.mapvoxels[-2::-1]:
-        #    modified_inds /= self.factor
-        #    # TODO slow but hopefully more likely to be correct way
-        #    ol._update_voxels(modified_inds, increments)
-        ##return modified_inds_finest
-
-    def removepoints(self, xyzs, return_removed=False):
-        return self.add_points(xyzs, increment=-1)
-            
-    # TODO Reconsider keyframe usage after trajectory class is implemented
-    #def setkeyframe(self, xyzs, pose):
-    #    xyzs = poseutil.transformPoints(xyzs,pose)
-    #    voxel_array = occupiedlist.pointstovoxels(xyzs, self.getresolution(), unique=True)
-    #    self.keyframe = set(occupiedlist.hashrows(voxel_array))
-    #    
-    #def getkeyframeset(self):
-    #    return self.keyframe
-        
-    def _sample_points(self, xyzs, sample_size):
-        start_resolution = self.getfinest().resolution
-        sample = util.volumetricsample(xyzs, start_resolution)
-        for i in range(1, 10):
-            sample_res = start_resolution * 2**i
-            sample = util.volumetricsample(sample, sample_res)
-            if len(sample) < sample_size:
-                break
-
-        if verbose:
-            print 'Volumetric sample resolution:', sample_res
-        return sample
-
-    # Ray trace methods
-    def calcray(self, ray):    
-        ol = self.getfinest()
-        hits = ol.segment_ovl(None, ray)[0] 
-        if len(hits) > 0:
-            vox_hits = occupiedlist.pointstovoxels(hits,ol.resolution,ol.lattice)
-            return quantizer.uniquerows(vox_hits, 0)
-        else:
-            return []
-            
-    def raytrace(self, pose, length, axis=0):
-        '''
-        perform a ray-trace from the pose and return all map voxels at the
-        base resolution along the ray.
-        It traces from any point in the map frame (e.g. the sensor's
-        current position) along a ray oriented as per the pose's angular 
-        components.
-        '''
-        res = self.getresolution()/2.
-        numcells = np.ceil(length/res)+2 # + 1 for 0th point + 1 for end point with poor aligned voxels
-        ray = np.zeros([numcells,3])
-        ray[:,axis] = np.hstack([0,np.cumsum(np.ones(numcells-1))*res])
-        ray = poseutil.transformPoints(ray, pose)[0]
-        return self.calcray(ray)
-        
-    def raytracepts(self, point1, point2, return_extras=False):
-        '''
-        perform a ray-trace from point1 to point2 and return all map 
-        voxels at the base resolution along the ray.
-        '''
-        res = self.getresolution()#/3.
-        point2a = point2 - point1
-        range2 = np.sum(point2a**2)
-        length = np.sqrt(range2)
-        dir_unit = point2a/length
-        numcells = int(np.ceil(length/res)+2) # + 1 for 0th point + 1 for last point
-        ray = np.zeros([numcells,3])
-        ## TODO make this a list comprehension as this loop slows the cast down lots
-        #for i in range(numcells):
-        #    ray[i] = dir_unit*i*res
-        ray_res = length/(numcells-1)
-        ray = fast.fillray(numcells, dir_unit, ray_res, ray)
-        ray = ray + point1
-        if return_extras:
-            return self.calcray(ray), length, dir_unit, ray
-        else:
-            return self.calcray(ray)
-        
-    # Alignment methods
-    def objectivefuncMROL(self, pose, xyzs): # TODO not really used anymore?
-        '''
-        Objective function to be minimised.
-        For a list of xyzs, transforms by pose and calculations the cost
-        at the various resolutions. If level is specified, only 
-        calc at that level.
-        '''
-        levels = len(self.mapvoxels)
-        hits = np.empty(levels)
-        resolutions = np.empty(levels)
-        for i, ol in enumerate(self.mapvoxels):
-            hits[i] = ol.calccollisions(pose, xyzs, query=False)
-            resolutions[i] = ol.resolution
-        # sort hits from coarse to fine
-        new = np.hstack((-np.diff(hits), hits[-1]))
-        #weights = np.ones(levels)
-        weights = resolutions[-1]/resolutions # same as ..., 1/8, 1/4, 1/2, 1
-        #cost = -np.dot(weights, hits)
-        cost = -np.dot(weights, new)
-        return cost
-     
-    def optimise_alignment(self, xyzs, initialpose, full_data=False):
-        dtheta_base = np.radians(0.5)
-        # TODO worried that this is not checking for adjacent voxel occupancy
-
-        # sample scan points appropriately
-        # no point doing multiple look ups on the same voxel and in fact an
-        # unfair weighting
-        # you want a volumetric sample such that the number of sample points is enough
-
-        # cast points to float32 for speed
-        xyzs = np.float32(xyzs)
-        xyzs_sample = self._sample_points(xyzs, sample_size)
-        #xyzs_sample = util.volumetricsample(xyzs, 0.4)
-
-        if verbose: 
-            print 'Number of sample points: %d' % len(xyzs_sample)
-
-        bestpose = initialpose.copy()
-        for level, ol in enumerate(self.mapvoxels):
-            objective_func = ol.cost_func 
-            dtheta = dtheta_base*pow(self.factor,(len(self.mapvoxels)-level-1))
-            dx = ol.resolution
-            # This helps avoid local minima, but limits accuracy to voxel size 
-            # at the finest resolution
-            if verbose:
-                print 'resolution: %f' % (ol.resolution)
-            # objective_func is -occupiedlist.calccollisions(pose, xyzs)
-            bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs_sample,), dx, dtheta, max_iterations=20, verbosity=2)
-            #bestpose, cost = optimization._cost_min_scipy(objective_func, bestpose, (xyzs_sample,))
-        # run it again at the finest resolution with tighter steps to get 
-        # sub-voxel accuracy.
-        dx = dx*pow(self.factor,-2)
-        dtheta = dtheta_base*pow(self.factor,-2)
-        if not full_data:
-            # Sometimes get better results with the below resampling, but much faster.
-            xyzs = util.offsetvolsample(xyzs, self.getfinest().resolution)
-        bestpose, cost = optimization.cost_min(objective_func, bestpose, (xyzs,), dx, dtheta, max_iterations=100, verbosity=2)
-        return bestpose, cost
-
-    def match(self, xyzs, centrepose, spreadpose):
-        """Does a global search of the specified pose space.
-        Find the possible locations of the occupied list ol in the current 
-        occupied list.  Takes an occupied list to match to the current one and 
-        two poses."""
-
-        # determines the orientation step size as it relates to occupancy list 
-        # resolution
-        poses = None
-        bestpose = poseutil.Pose3D()
-
-        totaltime = time.time()
-        lasttime = time.time()
-        # TODO think about this number
-        #xyzs = util.getsample(xyzs, 1000)
-
-        levels = len(self.mapvoxels)
-        table = np.zeros((levels+2, 9), dtype=np.dtype('S8'))
-        # header for the table of values of interest
-        table = []
-        table.append('Res modal map    scan   pose  pose  Top     Time  Best'.split())
-        table.append('(m) Frac  voxels points count count overlap Taken pose'.split())
-        table.append('--- ----- ------ ------ ----- ----- ------- ----- ----'.split())
-        for i, mapol in enumerate(self.mapvoxels):
-            tablerow = []
-            res = mapol.resolution
-            deltaposition = res
-            deltaorientation = res/self.feature_range # 8
-            if poses == None:
-                poses = poseutil.uniformposes(centrepose, spreadpose, deltaposition, deltaorientation)
-            else:
-                poses = poseutil.refineposes(poses, deltaposition, deltaorientation)
-            if verbose: 
-                print 'Number of poses to test:', len(poses)
-
-            # TODO remove poses outside search space this needs tidying up/thinking about
-            #cparr = np.array(centrepose.get())
-            #sparr = np.array(spreadpose.get())
-            #D = np.array((deltaposition, deltaposition, deltaposition, deltaorientation, deltaorientation, deltaorientation))
-            #maxpose = cparr + sparr + D
-            #minpose = cparr - sparr - D
-            #select = np.logical_and(np.all(poses <= maxpose, 1), np.all(poses >= minpose, 1))
-            ##print 'Culling poses: ', len(poses), sum(select)
-            #poses = poses[select]
-            
-            xyzs_sample = util.volumetricsample(xyzs, res)
-            tablerow.extend((res, modalfractions[i], len(mapol), len(xyzs_sample), len(poses)))
-
-            overlaps = []
-            for pose in poses: #  TIME consuming loop
-                #overlap = np.sum(mapol.calccollisions(pose, xyzs, query=True))
-                overlap = mapol.calccollisions(pose, xyzs_sample, query=False)
-                overlaps.append(overlap)
-                #overlaps.append(-self.objectivefuncMROL(pose, xyzs_sample))
-            overlaps = np.asarray(overlaps)
-
-            ind_max = np.argmax(overlaps)
-            bestpose.set(poses[ind_max])
-            bestoverlap = overlaps[ind_max]
-            candidates = overlaps >= modalfractions[i] * bestoverlap
-            # maybe just take the top n?
-            #if len(overlaps) > topn:
-            #    candidates = overlaps.argsort()[-topn:]
-            #else:
-            #    candidates = range(len(overlaps))
-
-            timetaken = time.time() - lasttime
-            lasttime = time.time()
-            tablerow.extend((len(candidates), bestoverlap, timetaken, bestpose))
-            table.append(tablerow)
-            print
-            printtable(table, width=9)
-            print 'Bestpose: ', bestpose
-            #scansize = ol.voxels[-1].shape[0]
-            poses = poses[candidates]
-
-        print "Total time: ", time.time() - totaltime
-        overlaps = overlaps[candidates]
-        return bestpose, bestoverlap
-        
-    def matchbest(self, xyzs, centrepose, spreadpose):
-        poses, overlaps = self.match(xyzs, centrepose, spreadpose)
-        bestind = np.argmax(overlaps)
-        bestpose = poseutil.Pose3D(poses[bestind, :])
-        cost = overlaps[bestind]
-        return bestpose, cost
-    
-    def matchandadd(self, ol, centrepose, spreadpose):
-        """ matches the given occupiedlist ol by searching around the 
-        centrepose a distance of spreadpose. If the match is successful the 
-        occupied list is transfomred and combined with the current occupied 
-        list. Returns the pose at which the occupied list ol was added."""
-        # TODO add a threshold for deciding whether to merge 
-        # TODO add unit test for this
-        # TODO add a ray trace clear?
-        poses, overlaps = self.match(ol, centrepose, spreadpose)
-        bestpose = poseutil.Pose3D(poses[np.argmax(overlaps), :])
-        self.add_points(bestpose.transformPoints(ol.getpoints()))
-        return bestpose
-

=== added file 'mrol_mapping/occupiedlist.py'
--- mrol_mapping/occupiedlist.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/occupiedlist.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,650 @@
+#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
+#
+#This program is distributed in the hope that it will be useful,
+#but WITHOUT ANY WARRANTY; without even the implied warranty of
+#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#GNU Lesser General Public License for more details.
+#
+#You should have received a copy of the GNU Lesser General Public License
+#along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+from __future__ import division
+import numpy as np
+import scipy.stats as stats
+
+import itertools
+import poseutil
+import quantizer
+import pointcloud
+from bresenhamline import bresenhamline
+import log
+import logging
+
+#import numexpr as ne
+#ne.set_num_threads(4)
+
+logger = log.getLogger('occupiedlist')
+logger.setLevel(logging.INFO)
+NBACKTRACE_VOXELS = 10
+
+inds = np.indices((3,3,3)) - 1
+inds.shape = (3, 27)
+inds = inds.T
+offset_inds = np.zeros((27, 4), dtype=np.int16)
+offset_inds[:,:3] = inds
+
+latticefuncs = {'cubic':quantizer.quantize_cubic, 'bcc':quantizer.quantize_bcc, 'fcc':quantizer.quantize_fcc}
+""" Dictionary of the currently implemented lattice quantizers """
+
+cdf_res = 0.01
+cdf_halfwidth = 2
+def _x_to_ind(X):
+    '''Converts x to an index in the lookup table'''
+    return np.int32((X + cdf_halfwidth)/cdf_res + 0.5)
+    
+normcdf = stats.norm.cdf(np.arange(-cdf_halfwidth, cdf_halfwidth, cdf_res))
+# lookup table for the cdf
+def cdf_approx(xs, sigma):
+    return normcdf[_x_to_ind(xs)]
+
+def loadxyz(fname, resolution):
+    '''loads a list of points in x, y, z format from a file and makes an 
+    occupied voxel list'''
+    xyzs = np.loadtxt(fname)
+    ol = OccupiedList(resolution)
+    ol.add_points(xyzs[:, 0:3])
+    return ol
+
+def pointstovoxels(X, resolution, pose=None, latticetype='cubic', out=None):
+    '''Converts a list of points to integer voxel indices. Optionally
+    transforming by pose as well for speed.'''
+    # Possible lattice types currently include cubic, bcc and fcc.
+    # TODO out of bound check required.
+    #return latticefuncs[latticetype](X, resolution)
+    if out is None: 
+        out = np.empty((X.shape[0], 4), dtype=np.int16) # uninitialised for speed
+        out[:,3] = 0 # set last column to zero
+    else: # make sure it is unpacked
+        _unpack(out)
+
+    # combines transformation and scaling into one operation for speed
+    X, pose = poseutil.transformPoints(X, pose, scale=1.0/resolution)
+    #out[:, :3] = np.round(X)
+    # out[:, :3] = X + 0.5 # this does not work quite right
+    # out[:, :3] = np.floor(X) # without multi-resolution offsets
+    out[:, :3] = np.floor(X + 0.5) # with multi resolution offsets but they do not seem to help that much
+    return out
+
+def voxelstopoints(A, resolution):
+    """ Converts an array of integers voxel indices to an array of points """
+    # TODO make this work for alternative lattices
+    return A*resolution
+    #return (A + 0.5)*resolution # without multi-resolution offsets
+
+def dilate(voxels):
+    '''Add adjacent voxels, effectively blurring or smearing occupied voxel 
+    list'''
+    # blur/smear map instead of line above to include checking of adjacent voxels
+    mg  = np.mgrid[-1:2, -1:2, -1:2]
+    mg.shape = (3, 27, 1)
+    voxels = np.array(voxels).T
+    dilated = mg + voxels[:, np.newaxis, :]
+    dilated = dilated.transpose((0, 2, 1)).reshape(3, -1).T
+    return np.array(tuple(set(tuplelist(dilated))))
+
+def tuplelist(X):
+    '''Converts an nx3 array to a list of tuples quickly this is useful for 
+    bulk insertion into dictionaries and sets'''
+    return zip(X[:, 0], X[:, 1], X[:, 2])
+
+def _4_column_int16(inds):
+    '''converts N x 3 array of voxel ints to N x 4 array of int16 with last
+    column 0'''
+    X_int16 = np.empty((inds.shape[0], 4), dtype=np.int16)
+    X_int16[:, :3] = inds
+    # 4th column needs to be zeros
+    X_int16[:, 3] = 0
+    #X_int16.dtype = np.int64
+    #X_int16.shape = -1
+    return X_int16
+
+def _unpack(ids):
+    '''converts an int64 array of voxel ids to an nx4 array of np.int16. 
+    Near instantaneous in place operation'''
+    ids.dtype = np.int16
+    ids.shape = -1, 4
+
+def _pack(inds):
+    '''converts in place an integer array of Nx4 int16 voxels to an array of
+    ids.  the last column are zeros.'''
+    assert inds.dtype == np.int16, 'inds needs to be np.int16'
+    assert inds.shape[1] == 4 
+    assert not np.isfortran(inds), 'Incorrect array memory layout'
+
+    inds.dtype = np.int64
+    inds.shape = -1
+    
+def _fast_count(ints, return_index=False):
+    #counted_ids = collections.Counter(ids) # collections.Counter turns out to be slow
+
+    # Fast counting of integers although might not scale to large N well
+    # because of the sort done by unique?
+    if return_index:
+        uniqs, uniqinds, inverseinds = np.unique(ints, return_index=True,
+                                          return_inverse=True)
+        counts = np.bincount(inverseinds)
+        return uniqs, counts, uniqinds
+    else:
+        uniqs, inds = np.unique(ints, return_inverse=True)
+        counts = np.bincount(inds)
+        return uniqs, counts
+
+class BloomFilter:
+    '''Special case counting bloom filter optimized for voxel indices'''
+
+    def __init__(self, size):
+        # TODO add probability of false positive as parameter
+        self.size = int(6*size) # should this be prime or what?
+        self.K = 2 # number of hash functions 
+        self.bloomtable = np.zeros(self.size, dtype=np.uint16) 
+        # signed here although not needed makes update errors to the bloomtable 
+        # easier to spot
+
+    def __len__(self): 
+        return self.size
+
+    def __eq__(self, bf):
+        return np.all(self.bloomtable == bf.bloomtable)
+
+    def djbhash(self, i):
+        '''Hash function from D J Bernstein'''
+        h = 5381L
+        t = (h * 33) & 0xffffffffL
+        h = t ^ i
+        return h
+
+    def fnvhash(self, i):
+        '''Fowler, Noll, Vo Hash function'''
+        h = 2166136261
+        t = (h * 16777619) & 0xffffffffL
+        h = t ^ i
+        return h
+
+    #mask1 = eval('0b' + 32 * '01')
+    #mask2 = eval('0b' + 32 * '10')
+    def _hash_voxel_ids(self, ids):
+        # Need a hash function that that can generate multiple hashes for given 
+        # input and over a specified range
+        inds = np.empty((2, ids.shape[0]), np.int64)
+        # use id values themselves as hash dangerous and quicker but when benchmarked seemed to make little difference
+        # inds[0] = ids 
+        inds[0] = self.fnvhash(ids)
+        inds[1] = self.djbhash(ids)
+        S = self.size
+        #return ne.evaluate('inds % S') 
+        return inds % self.size
+        
+    def add_voxel_ids(self, ids):
+        inds = self._hash_voxel_ids(ids)
+        for i in range(self.K):
+            self.bloomtable[inds[i]] +=1
+
+    def remove_voxel_ids(self, ids):
+        '''Decrement the counts for this counting bloom table but clamp to 
+        zero'''
+        inds = self._hash_voxel_ids(ids)
+        for i in range(self.K):
+            remove_inds = inds[i][self.bloomtable[inds[i]] > 0]
+            for ind in remove_inds:
+                # select only those inds with counts > 0
+                self.bloomtable[ind] -= 1
+                #if np.any(to_change < 1):
+                # only decrement those entries which are > 0
+
+    def contains(self, ids):
+        '''Note that this is an approximation with tunable error.  False 
+        positives are possible but false negatives are not.'''
+
+        # TODO currently only works for 2 hash functions
+
+        # create K indices into bloom table from K hash functions
+        inds = self._hash_voxel_ids(ids)
+
+        # TODO surely this can be done in a more efficient manner?
+        present = np.zeros((self.K, len(ids)), dtype=bool)
+        for i in range(self.K):
+            present[i] = self.bloomtable[inds[i]] > 0
+
+        #allpresent = present.sum(axis=0) == self.K
+        #allpresent = np.all(present, axis=0)
+
+        #a = present[0]
+        #b = present[1]
+        #allpresent = ne.evaluate('a & b')
+        #return np.all(present, axis=0)
+        return np.logical_and(present[0], present[1])
+        
+def user_add(cur_data,data):
+    ''' An example of a user add function for incrementing user data in 
+    a map voxel given a new measurement'''
+    # First we must handle the case where this is the first time we've 
+    # added data to the voxel, easily identified because the current
+    # voxel data only contains the number of prior observations (zero).
+    if len(cur_data) == 0:
+        return data
+    else:
+        # This should be customised to the user's application. Typically 
+        # it should check for consistency in data length also
+        return np.int64((np.array(cur_data) + np.array(data)) / 2.) # some merging algorithm
+        
+class default_vox_content:
+    # TODO consider replacing this with a np.recarray or some numpy.dtype trickery?
+    def __init__(self):
+        self.data = np.array([0],dtype=int)
+    
+    def __getitem__(self, index):
+        if index == 0:
+            return self.data
+        else:
+            return np.array([])
+            
+class OccupiedList:
+    """ stores a set of voxel indices list at a single resolution """
+
+    def __init__(self, resolution, maxvoxels=1e6, use_bloom=True):
+        self.resolution = resolution
+        self._transformedvoxels = None
+        self.voxdtype = None
+
+        # voxels is a dictionary with keys of np.int64 IDs that are the result
+        # of packing 4 np.int16
+        # the content of the dictionary is either an np.int16 holding
+        # increments, or TODO a userdefined structure
+        self._voxels = dict()
+        self.lattice = 'cubic' # should be one of the latticefuncs
+        self._use_bloom = use_bloom
+        self.use_KDE = False
+        if self.use_KDE:
+            # Worst case scenario for a cubic lattice is a point right on a
+            # diagonal corner, i.e. the probability is spread evenly between 8
+            # cells (0.125). It seems reasonable to curtail the occupancy if it
+            # is half of this probability (1/4 of a cell diagonal if the cdf
+            # was linear).
+            self.occupancy_threshold = 0.5 * 1/8
+        else:
+            self.occupancy_threshold = 1
+            # below which voxels are removed and changes/increments of these
+            # are ignored unless re-added occupancy scale factor 
+        
+        self.maxvoxels = maxvoxels
+        if self._use_bloom:
+            self.bloomfilter = BloomFilter(self.maxvoxels)
+
+    def __eq__(self, o):
+        if self.resolution != o.resolution or len(self) != len(o):
+            return False
+        for K, V in self._voxels.items():
+            if o._voxels[K] != V:
+                return False
+        return True
+
+    def __len__(self):
+        return len(self._voxels)
+
+    def __repr__(self): 
+        # TODO: rewrite considering userdata
+        occupancies = np.asarray(self._voxels.values())
+        occupancies.shape = -1, 1
+        X = np.hstack((self.getvoxels(), occupancies))
+        return str(X)
+
+    def __sub__(self, ol):
+        '''Set like subtraction'''
+        # TODO in python 2.7 you can dict1.viewkeys() & dict2.viewkeys() to get 
+        # the intersection of the keys on dictionary
+        # TODO maybe quicker would be to find the intersection and then delete 
+        # those voxels
+        self_set = set(self._voxels.keys())
+        ol_set = set(ol._voxels.keys())
+        D = np.fromiter(self_set - ol_set, dtype=np.int64)
+        _unpack(D)
+        Dol = OccupiedList(self.resolution)
+        increments = np.ones(len(D))
+        Dol._update_voxels(D, increments)
+        return Dol
+    
+    # TODO These methods of get and set should be moved to a more generic sparse nD structure
+    def set_occupancies(self, voxel_inds, occupancies):
+        '''
+        Sets the specified voxel indices to the occupancies given.
+
+        >>> ol = OccupiedList(0.1)
+        >>> inds = np.array(((1,2,3), ))
+        >>> ol.set_occupancies(inds, (1,2))
+        >>> ol
+        [[1 2 3 1]]
+
+        '''
+        ids = _4_column_int16(voxel_inds)
+        _pack(ids)
+        self._check_voxdtype(None)
+        for ID, occ in zip(ids, occupancies):
+            self._voxels[ID] = occ
+
+    def get_occupancies(self, voxel_inds):
+        '''
+        Gets the occupancies for the given voxel indices
+
+        >>> ol = OccupiedList(0.1)
+        >>> n = 100
+        >>> inds = np.random.randint(-9999, 9999, (n,3))
+        >>> occs = np.random.randint(0, 1000, n)
+        >>> ol.set_occupancies(inds, occs)
+        >>> np.all(ol.get_occupancies(inds) == occs)
+        True
+
+        Getting empty voxels should not change ol
+        
+        >>> ol = OccupiedList(0.1)
+        >>> ol.get_occupancies( np.array(((1,2,3),)) )
+        array([0])
+        >>> len(ol)
+        0
+        '''
+        ids = _4_column_int16(voxel_inds)
+        _pack(ids)
+        return np.array([self._voxels.get(ID, 0) for ID in ids])
+
+    def getvoxels(self):
+        ids = np.asarray(self._voxels.keys())
+        _unpack(ids)
+        return ids[:,:3]
+        
+    def getvoxeldata(self):
+        '''Returns both voxel indices and assocated data'''
+        voxels = self.getvoxels() * self.resolution
+        if not len(voxels):
+            return voxels
+        voxdtype = self._voxels.itervalues().next().dtype
+        if ('userdata' in voxdtype.names):
+            vox_val = np.array(self._voxels.values(), dtype=voxdtype)
+            voxels = np.hstack([voxels, vox_val['userdata']])
+        return voxels
+
+    def getpoints(self):
+        return voxelstopoints(self.getvoxels(), self.resolution)
+
+    def display(self):
+        pc = pointcloud.PointCloud(self.getpoints())
+        pc.display()
+    
+    def KDE(self, x0, a0, sigma):
+        '''Does a kernel density estimate for the point, x0 should be lattice 
+        units and a0 is the coordinates of the nearest lattice point in lattice 
+        units'''
+        # calculate the kernel integrals between -inf, x0, x0 + 1 and inf
+        integrals = np.zeros((3,3))
+        for i, x in enumerate(x0):
+            a = a0[i]
+            #cumulative_integrals = stats.norm.cdf((a - 0.5, a + 0.5), loc=x, scale=sigma)
+            cumulative_integrals = cdf_approx(np.asarray((a - x - 0.5, a - x + 0.5)), sigma=sigma)
+            integrals[i] = np.diff(np.hstack((0, cumulative_integrals, 1)))
+        # outer product returning a 3 * 3 * 3 array
+        X = integrals[0].reshape(3,1,1)
+        Y = integrals[1].reshape(1,3,1)
+        Z = integrals[2].reshape(1,1,3)
+        # return the kernel density integral over the volume of the lattice 
+        # cell
+        return X * Y * Z
+
+    # this function is not used anymore but is kept for reference when adding
+    # capability to store other data such as RGB.
+    def _update_voxel_func_user(self, ID, increment, userdata):
+        vox = self._voxels[ID]
+        self._voxels[ID] = np.hstack([vox[0] + increment, self.useraddfn(vox[1:], userdata)])
+        if self._voxels[ID][0] < self.occupancy_threshold:
+            del self._voxels[ID]
+            self.removed_ids.append(ID)
+
+    def _extract_voxdtype(self, userdata):
+        if userdata is not None:
+            # remove duplicates from userdata
+            voxdtype = np.dtype([('count', int),
+                                 ('userdata',
+                                  userdata.dtype, userdata.shape[1])
+                                ])
+        else:
+            if len(self._voxels):
+                # make zeros consistent with existing self._voxels
+                firstval = self._voxels.itervalues().next()
+                voxdtype = firstval.dtype
+            else:
+                voxdtype = np.dtype([('count', int)])
+        return voxdtype
+
+    def _check_voxdtype(self, userdata):
+        voxdtype = self._extract_voxdtype(userdata)
+        if self.voxdtype is None:
+            self.voxdtype = voxdtype
+        else:
+            assert voxdtype == self.voxdtype
+
+    def _zeros(self):
+        return np.zeros(1, dtype=self.voxdtype)[0] 
+        
+    def _update_voxels(self, increment_inds, increment, userdata=None):
+        #ids = _4_column_int16(increment_inds)
+        _pack(increment_inds)
+
+        # could just do the line below however need the added ones for bloom
+        # filter and so that voxels with zero occupancy are removed
+        # self._voxels.update(increment_inds)
+
+        # Generate a list of newly added voxels and removed voxels for the 
+        # bloom table update, and do any voxel merging operations
+        logger.info('Number of points to add: %d' % len(increment_inds))
+        # merging multiple increments of the same voxel
+        # count number of added points for each voxel
+
+        uniqs, counts, uniqinds = _fast_count(increment_inds,
+                                              return_index=True)
+        if userdata is not None:
+            userdata = userdata[uniqinds]
+        # uniqs, counts are the unique values and their corresponding counts
+        self._check_voxdtype(userdata)
+
+        # perhaps a hack to get zero value
+        zeros = self._zeros()#np.zeros(1, dtype=voxdtype)[0] 
+        newcounts = counts * increment
+        value_uniqs = np.array([self._voxels.get(ID, zeros) for ID in uniqs],
+                               dtype=self.voxdtype)
+        occupancies = value_uniqs['count']
+
+        # counter values are initialized with zero. Non-zero values indicate
+        # pre-existance
+        existing_ids = (occupancies != zeros['count']).reshape(-1)
+        
+        # #############
+        # actual update
+        # #############
+        # add map occupancies with collected voxel counts of points to be added
+        if userdata is not None:
+            value_uniqs['userdata'] = userdata
+        value_uniqs['count'] = value_uniqs['count'] + newcounts
+        occupancies = value_uniqs['count']
+
+        # split into two groups, those that were changed and those that require
+        # removal
+
+        # delete any less than 1
+        inds = (occupancies < self.occupancy_threshold)
+        removed_ids = uniqs[inds]
+        removed_existing = uniqs[inds & existing_ids]
+
+        changed_ids = uniqs[np.logical_not(inds)]
+        changed_values = value_uniqs[np.logical_not(inds)]
+
+        logger.info("Removing:%d" % len(removed_existing))
+        for ID in removed_existing:
+            del self._voxels[ID]
+
+
+        # set those with occupancy 1 or more 
+        # set occupancies of voxels with their new values
+        self._voxels.update(itertools.izip(changed_ids, changed_values))
+
+        #for ID in increment_inds:
+        #    if ID not in self._voxels:
+        #        new_ids.append(ID)
+        #    update_func(ID, increment)
+        #    #self._update_voxel_func_user(ID, increments[i], userdata[i])
+
+        #self.removed_ids = np.asarray(self.removed_ids)
+        #new_ids = np.asarray(new_ids)
+        if self._use_bloom:
+            self.bloomfilter.add_voxel_ids(changed_ids)
+            self.bloomfilter.remove_voxel_ids(removed_existing) 
+
+    def clear_freespace(self, xyzs, sensor_location):
+        '''
+        Clear the voxels in global map that we are quite certain about not
+        being occupied.
+
+        Parameters:
+            xyzs : points with sensor at origin. The points have not been
+            transformed to global map coordinates. But the `pose` provides the
+            exact transformation required for converting to global map.
+            pose : The pose that will map the points `xyzs` to global map.
+        '''
+        if not len(self._voxels):
+            return
+        # TODO should really take a point cloud object rather than xyzs
+        if len(xyzs) == 0:
+            return
+        xyzs = np.atleast_2d(xyzs) # for single points
+        assert xyzs.shape[1] == 3, 'Points should be in a n by 3 array format'
+
+        inds = pointstovoxels(xyzs, resolution=self.resolution, latticetype=self.lattice)
+        sensor_voxel = pointstovoxels(sensor_location,
+                                      resolution=self.resolution, latticetype=self.lattice)
+
+        free_voxels = bresenhamline(inds, sensor_voxel,
+                                    max_iter=NBACKTRACE_VOXELS)
+
+        # equivalent of remove points
+        self._update_voxels(free_voxels, -1)
+        assert len(self._voxels) < self.maxvoxels, 'Max voxels exceeded for bloom table'
+
+
+    # TODO do add_points and removepoints properly by apportioning the point 
+    # probability amongst surrounding voxels, like kernel density estimation.
+    # This update should also be more sophisticated in terms of the sensor model.
+    # TODO specify sigma as a covariance matrix for each point to be added
+    # (this takes into account the pose of the sensor)
+    def add_points(self, xyzs, pose=None, userdata=None, increment=1, return_modified=False, sigma=0.5):
+        """ convert points to voxel indices and append them to this occupied 
+        voxel list """
+        # TODO should really take a point cloud object rather than xyzs
+        if len(xyzs) == 0:
+            return
+        xyzs = np.atleast_2d(xyzs) # for single points
+        assert xyzs.shape[1] == 3, 'Points should be in a n by 3 array format'
+        if pose is not None:
+            xyzs = pose.transformPoints(xyzs)
+
+        inds = pointstovoxels(xyzs, resolution=self.resolution, latticetype=self.lattice)
+        # TODO check that voxel indices are not out of bounds
+
+        if self.use_KDE:
+            # TODO is similar to mgrid used in dilate, refactor these together or 
+            # remove dilate as it is not needed now we are doing kernel density 
+            # estimation
+            surrounding_inds = np.int16((np.indices((3,3,3)) - 1).reshape(3, -1).T)
+
+            # Add voxels to be incremented according to kernel density estimation
+            increment_inds = []
+            increments = []
+            # TODO rather than use a list preallocate the array at 27 * len(xyzs)?
+            # TODO make this work for single points
+            for xyz, ind in zip(xyzs, inds):
+                #increments.extend(np.int8(increment*self.KDE(xyz/self.resolution, ind)).ravel())
+                kde = self.KDE(xyz/self.resolution, ind, sigma=sigma)
+                #increments.extend(np.int8(increment*kde).ravel())
+                increments.extend((increment*kde).ravel())
+                # Now add these increments to the appropriate indices
+                increment_inds.extend(ind + surrounding_inds)
+
+            increment_inds = np.asarray(increment_inds)
+            increments = np.asarray(increments)
+
+            # remove increments that are too small to consider
+            bigenough_inds = np.abs(increments) >= self.occupancy_threshold
+            increments = increments[bigenough_inds]
+            increment_inds = increment_inds[bigenough_inds]
+        else:
+            increment_inds = inds
+
+        self._update_voxels(increment_inds, increment, userdata)
+
+        assert len(self._voxels) < self.maxvoxels, 'Max voxels exceeded for bloom table'
+        return increment_inds
+
+    def removepoints(self, xyzs, decrement=1, return_removed=False, **args):
+        '''Decrement the observations in the voxels that the xyzs fall 
+        into.'''
+        return self.add_points(xyzs, pose=None, increment=-decrement, return_modified=return_removed, **args)
+
+        #If the delete flag is set, then remove the voxels that 
+        #xyzs fall into. Return both the voxels that xyzs fall into and the 
+        #total decrement amount for that voxel.
+
+    def savexyz(self, fname):
+        """ saves the finest occupied list to fname in x y z format """
+        np.savetxt(fname, self.getpoints())
+
+    def calccollisions(self, pose, xyzs, returntotal=True, query=False):
+        '''The intersection of points with this occupied voxel list.
+        If query then  return the density for each point after transformation 
+        by the given pose, rather than just a count of hits.  
+        '''
+        # reuse transformed voxels to save initialising the fourth column it to
+        # zero each time.  This is some nasty code I hope the speed increase is
+        # worth it.
+        if self._transformedvoxels is None or len(self._transformedvoxels) != len(xyzs):
+            self._transformedvoxels = pointstovoxels(xyzs, resolution=self.resolution, pose=pose, out=None)
+        else:
+            self._transformedvoxels = pointstovoxels(xyzs, resolution=self.resolution, pose=pose, out=self._transformedvoxels)
+
+        _pack(self._transformedvoxels)
+
+        if query:
+            return [self._voxels[ID] for ID in self._transformedvoxels]
+            #return sum(self._voxels[ID] for ID in self._transformedvoxels)
+
+        if self._use_bloom:
+            present = self.bloomfilter.contains(self._transformedvoxels)
+            if returntotal:
+                return np.sum(present)
+            else:
+                return present
+        else:
+            if returntotal:
+                return sum(ID in self._voxels for ID in self._transformedvoxels)
+            else:
+                return [ID in self._voxels for ID in self._transformedvoxels]
+
+    def cost_func(self, pose, xyzs):
+        return -self.calccollisions(pose, xyzs)
+
+    def segment_ovl(self, pose, xyzs):
+        ''' returns the inliers and outliers of the intersection of a point 
+        cloud with this occupied voxel list.'''
+        xyzs = poseutil.transformPoints(xyzs, pose)[0]
+        overlap_inds = np.array(self.calccollisions(None, xyzs, returntotal=False))
+        inliers = xyzs[overlap_inds]
+        outliers = xyzs[np.logical_not(overlap_inds)]
+        return inliers, outliers
+
+if __name__ == "__main__":
+    import doctest
+    doctest.testmod()

=== removed file 'mrol_mapping/occupiedlist.py'
--- mrol_mapping/occupiedlist.py	2012-03-09 15:29:52 +0000
+++ mrol_mapping/occupiedlist.py	1970-01-01 00:00:00 +0000
@@ -1,549 +0,0 @@
-#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
-#
-#This program is distributed in the hope that it will be useful,
-#but WITHOUT ANY WARRANTY; without even the implied warranty of
-#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#GNU Lesser General Public License for more details.
-#
-#You should have received a copy of the GNU Lesser General Public License
-#along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-from __future__ import division
-import collections
-import numpy as np
-import scipy.stats as stats
-
-import poseutil
-import quantizer
-import pointcloud
-
-#import numexpr as ne
-#ne.set_num_threads(4)
-
-verbose = False
-
-inds = np.indices((3,3,3)) - 1
-inds.shape = (3, 27)
-inds = inds.T
-offset_inds = np.zeros((27, 4), dtype=np.int16)
-offset_inds[:,:3] = inds
-
-""" Dictionary of the currently implemented lattice quantizers """
-latticefuncs = {'cubic':quantizer.quantize_cubic, 'bcc':quantizer.quantize_bcc, 'fcc':quantizer.quantize_fcc}
-
-cdf_res = 0.01
-cdf_halfwidth = 2
-def _x_to_ind(X):
-    '''Converts x to an index in the lookup table'''
-    return np.int32((X + cdf_halfwidth)/cdf_res + 0.5)
-    
-normcdf = stats.norm.cdf(np.arange(-cdf_halfwidth, cdf_halfwidth, cdf_res))
-# lookup table for the cdf
-def cdf_approx(xs, sigma):
-    return normcdf[_x_to_ind(xs)]
-
-def loadxyz(fname, resolution):
-    '''loads a list of points in x, y, z format from a file and makes an 
-    occupied voxel list'''
-    xyzs = np.loadtxt(fname)
-    ol = OccupiedList(resolution)
-    ol.add_points(xyzs[:, 0:3])
-    return ol
-
-def pointstovoxels(X, resolution=None, pose=None, latticetype='cubic', out=None):
-    '''Converts a list of points to integer voxel indices. Optionally
-    transforming by pose as well for speed.'''
-    # Possible lattice types currently include cubic, bcc and fcc.
-    # TODO out of bound check required.
-    #return latticefuncs[latticetype](X, resolution)
-    if out is None: 
-        out = np.empty((X.shape[0], 4), dtype=np.int16) # uninitialised for speed
-        out[:,3] = 0 # set last column to zero
-    else: # make sure it is unpacked
-        _unpack(out)
-
-    # combines transformation and scaling into one operation for speed
-    X, pose = poseutil.transformPoints(X, pose, scale=1.0/resolution)
-    #out[:, :3] = np.round(X)
-    out[:, :3] = X + 0.5
-    #out[:, :3] = np.floor(X+0.5)
-
-    return out
-
-def voxelstopoints(A, resolution):
-    """ Converts an array of integers voxel indices to an array of points """
-    # TODO make this work for alternative lattices
-    return A*resolution
-
-def dilate(voxels):
-    '''Add adjacent voxels, effectively blurring or smearing occupied voxel 
-    list'''
-    # blur/smear map instead of line above to include checking of adjacent voxels
-    mg  = np.mgrid[-1:2, -1:2, -1:2]
-    mg.shape = (3, 27, 1)
-    voxels = np.array(voxels).T
-    dilated = mg + voxels[:, np.newaxis, :]
-    dilated = dilated.transpose((0, 2, 1)).reshape(3, -1).T
-    return np.array(tuple(set(tuplelist(dilated))))
-
-def tuplelist(X):
-    '''Converts an nx3 array to a list of tuples quickly this is useful for 
-    bulk insertion into dictionaries and sets'''
-    return zip(X[:, 0], X[:, 1], X[:, 2])
-
-def _4_column_int16(inds):
-    '''converts N x 3 array of voxel ints to N x 4 array of int16 with last
-    column 0'''
-    X_int16 = np.empty((inds.shape[0], 4), dtype=np.int16)
-    X_int16[:, :3] = inds
-    # 4th column needs to be zeros
-    X_int16[:, 3] = 0
-    #X_int16.dtype = np.int64
-    #X_int16.shape = -1
-    return X_int16
-
-def _unpack(ids):
-    '''converts an int64 array of voxel ids to an nx4 array of np.int16. 
-    Near instantaneous in place operation'''
-    ids.dtype = np.int16
-    ids.shape = -1, 4
-
-def _pack(inds):
-    '''converts in place an integer array of Nx4 int16 voxels to an array of
-    ids.  the last column are zeros.'''
-    assert inds.dtype == np.int16, 'inds needs to be np.int16'
-    assert inds.shape[1] == 4 
-
-    inds.dtype = np.int64
-    inds.shape = -1
-    
-def _fast_count(ints):
-    #counted_ids = collections.Counter(ids) # collections.Counter turns out to be slow
-
-    # Fast counting of integers although might not scale to large N well
-    # because of the sort done by unique?
-    uniqs, inds = np.unique(ints, return_inverse=True)
-    counts = np.bincount(inds)
-    return uniqs, counts
-
-class BloomFilter:
-    '''Special case counting bloom filter optimized for voxel indices'''
-
-    def __init__(self, size):
-        # TODO add probability of false positive as parameter
-        self.size = int(6*size) # should this be prime or what?
-        self.K = 2 # number of hash functions 
-        self.bloomtable = np.zeros(self.size, dtype=np.uint16) 
-        # signed here although not needed makes update errors to the bloomtable 
-        # easier to spot
-
-    def __len__(self): 
-        return self.size
-
-    def __eq__(self, bf):
-        return np.all(self.bloomtable == bf.bloomtable)
-
-    def djbhash(self, i):
-        """Hash function from D J Bernstein"""
-        h = 5381L
-        t = (h * 33) & 0xffffffffL
-        h = t ^ i
-        return h
-
-    def fnvhash(self, i):
-        """Fowler, Noll, Vo Hash function"""
-        h = 2166136261
-        t = (h * 16777619) & 0xffffffffL
-        h = t ^ i
-        return h
-
-    #mask1 = eval('0b' + 32 * '01')
-    #mask2 = eval('0b' + 32 * '10')
-    def _hash_voxel_ids(self, ids):
-        # Need a hash function that that can generate multiple hashes for given 
-        # input and over a specified range
-        inds = np.empty((2, ids.shape[0]), np.int64)
-        inds[0] = self.fnvhash(ids)
-        inds[1] = self.djbhash(ids)
-        S = self.size
-        #return ne.evaluate('inds % S') 
-        return inds % self.size
-        
-    def add_voxel_ids(self, ids):
-        inds = self._hash_voxel_ids(ids)
-        for i in range(self.K):
-            self.bloomtable[inds[i]] +=1
-
-    def remove_voxel_ids(self, ids):
-        '''Decrement the counts for this counting bloom table but clamp to 
-        zero'''
-        inds = self._hash_voxel_ids(ids)
-        for i in range(self.K):
-            remove_inds = inds[i][self.bloomtable[inds[i]] > 0]
-            for ind in remove_inds:
-                # select only those inds with counts > 0
-                self.bloomtable[ind] -= 1
-                #if np.any(to_change < 1):
-                # only decrement those entries which are > 0
-
-    def contains(self, ids):
-        '''Note that this is an approximation with tunable error.  False 
-        positives are possible but false negatives are not.'''
-
-        # TODO currently only works for 2 hash functions
-
-        # create K indices into bloom table from K hash functions
-        inds = self._hash_voxel_ids(ids)
-
-        # TODO surely this can be done in a more efficient manner?
-        present = np.zeros((self.K, len(ids)), dtype=bool)
-        for i in range(self.K):
-            present[i] = self.bloomtable[inds[i]] > 0
-
-        #allpresent = present.sum(axis=0) == self.K
-        #allpresent = np.all(present, axis=0)
-
-        #a = present[0]
-        #b = present[1]
-        #allpresent = ne.evaluate('a & b')
-        #return np.all(present, axis=0)
-        return np.logical_and(present[0], present[1])
-        
-def user_add(cur_data,data):
-    ''' An example of a user add function for incrementing user data in 
-    a map voxel given a new measurement'''
-    # First we must handle the case where this is the first time we've 
-    # added data to the voxel, easily identified because the current
-    # voxel data only contains the number of prior observations (zero).
-    if len(cur_data) == 0:
-        return data
-    else:
-        # This should be customised to the user's application. Typically 
-        # it should check for consistency in data length also
-        return np.int64((np.array(cur_data) + np.array(data)) / 2.) # some merging algorithm
-        
-class default_vox_content:
-    # TODO consider replacing this with a np.recarray or some numpy.dtype trickery?
-    def __init__(self):
-        self.data = np.array([0],dtype=int)
-    
-    def __getitem__(self, index):
-        if index == 0:
-            return self.data
-        else:
-            return np.array([])
-            
-class OccupiedList:
-    """ stores a set of voxel indices list at a single resolution """
-
-    def __init__(self, resolution, maxvoxels=1e6, use_bloom=True):
-        self.resolution = resolution
-        self._transformedvoxels = None
-
-        # voxels is a dictionary with keys of np.int64 IDs that are the result
-        # of packing 4 np.int16
-        # the content of the dictionary is either an np.int16 holding
-        # increments, or TODO a userdefined structure
-        self._voxels = collections.Counter()
-        self.lattice = 'cubic' # should be one of the latticefuncs
-        self._use_bloom = use_bloom
-        self.use_KDE = False
-        if self.use_KDE:
-            # Worst case scenario for a cubic lattice is a point right on a
-            # diagonal corner, i.e. the probability is spread evenly between 8
-            # cells (0.125). It seems reasonable to curtail the occupancy if it
-            # is half of this probability (1/4 of a cell diagonal if the cdf
-            # was linear).
-            self.occupancy_threshold = 0.5 * 1/8
-        else:
-            self.occupancy_threshold = 1
-            # below which voxels are removed and changes/increments of these
-            # are ignored unless re-added occupancy scale factor 
-        
-        self.maxvoxels = maxvoxels
-        if self._use_bloom:
-            self.bloomfilter = BloomFilter(self.maxvoxels)
-
-    def __eq__(self, o):
-        if self.resolution != o.resolution or len(self) != len(o):
-            return False
-        for K, V in self._voxels.items():
-            if o._voxels[K] != V:
-                return False
-        return True
-
-    def __len__(self):
-        return len(self._voxels)
-
-    def __repr__(self): 
-        # TODO: rewrite considering userdata
-        occupancies = np.asarray(self._voxels.values())
-        occupancies.shape = -1, 1
-        X = np.hstack((self.getvoxels(), occupancies))
-        return str(X)
-
-    def __sub__(self, ol):
-        '''Set like subtraction'''
-        # TODO in python 2.7 you can dict1.viewkeys() & dict2.viewkeys() to get 
-        # the intersection of the keys on dictionary
-        # TODO maybe quicker would be to find the intersection and then delete 
-        # those voxels
-        self_set = set(self._voxels.keys())
-        ol_set = set(ol._voxels.keys())
-        D = np.fromiter(self_set - ol_set, dtype=np.int64)
-        _unpack(D)
-        Dol = OccupiedList(self.resolution)
-        increments = np.ones(len(D))
-        Dol._update_voxels(D, increments)
-        return Dol
-    
-    # TODO These methods of get and set should be moved to a more generic sparse nD structure
-    def set_occupancies(self, voxel_inds, occupancies):
-        '''
-        Sets the specified voxel indices to the occupancies given.
-
-        >>> ol = OccupiedList(0.1)
-        >>> inds = ((1,2,3), )
-        >>> ol.set_occupancies(inds, (1,2))
-        >>> ol
-        [[1 2 3 1]]
-
-        '''
-        ids = _4_column_int16(voxel_inds)
-        _pack(ids)
-        for ID, occ in zip(ids, occupancies):
-            self._voxels[ID] = occ
-
-    def get_occupancies(self, voxel_inds):
-        '''
-        Gets the occupancies for the given voxel indices
-
-        >>> ol = OccupiedList(0.1)
-        >>> n = 100
-        >>> inds = np.random.randint(-9999, 9999, (n,3))
-        >>> occs = np.random.randint(0, 1000, n)
-        >>> ol.set_occupancies(inds, occs)
-        >>> np.all(ol.get_occupancies(inds) == occs)
-        True
-
-        Getting empty voxels should not change ol
-        
-        >>> ol = OccupiedList(0.1)
-        >>> ol.get_occupancies( ((1,2,3),) )
-        array([0])
-        >>> len(ol)
-        0
-        '''
-        ids = _4_column_int16(voxel_inds)
-        _pack(ids)
-        return np.array([self._voxels[ID] for ID in ids])
-
-    def getvoxels(self):
-        ids = np.asarray(self._voxels.keys())
-        _unpack(ids)
-        return ids[:,:3]
-        
-    def getvoxeldata(self):
-        '''Returns both voxel indices and assocated data'''
-        return self.getvoxels(), np.array(self._voxels.values())
-
-    def getpoints(self):
-        return voxelstopoints(self.getvoxels(), self.resolution)
-
-    def display(self):
-        pc = pointcloud.PointCloud(self.getpoints())
-        pc.display()
-    
-    def KDE(self, x0, a0, sigma):
-        '''Does a kernel density estimate for the point, x0 should be lattice 
-        units and a0 is the coordinates of the nearest lattice point in lattice 
-        units'''
-        # calculate the kernel integrals between -inf, x0, x0 + 1 and inf
-        integrals = np.zeros((3,3))
-        for i, x in enumerate(x0):
-            a = a0[i]
-            #cumulative_integrals = stats.norm.cdf((a - 0.5, a + 0.5), loc=x, scale=sigma)
-            cumulative_integrals = cdf_approx(np.asarray((a - x - 0.5, a - x + 0.5)), sigma=sigma)
-            integrals[i] = np.diff(np.hstack((0, cumulative_integrals, 1)))
-        # outer product returning a 3 * 3 * 3 array
-        X = integrals[0].reshape(3,1,1)
-        Y = integrals[1].reshape(1,3,1)
-        Z = integrals[2].reshape(1,1,3)
-        # return the kernel density integral over the volume of the lattice 
-        # cell
-        return X * Y * Z
-
-    # this function is not used anymore but is kept for reference when adding
-    # capability to store other data such as RGB.
-    def _update_voxel_func_user(self, ID, increment, userdata):
-        vox = self._voxels[ID]
-        self._voxels[ID] = np.hstack([vox[0] + increment, self.useraddfn(vox[1:], userdata)])
-        if self._voxels[ID][0] < self.occupancy_threshold:
-            del self._voxels[ID]
-            self.removed_ids.append(ID)
-        
-    def _update_voxels(self, increment_inds, increment, userdata=None):
-        #ids = _4_column_int16(increment_inds)
-        _pack(increment_inds)
-
-        # could just do the line below however need the added ones for bloom
-        # filter and so that voxels with zero occupancy are removed
-        # self._voxels.update(increment_inds)
-
-        # Generate a list of newly added voxels and removed voxels for the 
-        # bloom table update, and do any voxel merging operations
-        if verbose:
-            print 'Number of points to add: ', len(increment_inds)
-        # merging multiple increments of the same voxel
-        # count number of added points for each voxel
-
-        uniqs, counts = _fast_count(increment_inds)
-        # uniqs, counts are the unique values and their corresponding counts
-
-        # TODO there is probably a bug if increment is 0 so remove increments of zero
-
-        # query the map for occupancies of these voxels
-        occupancies = np.array([self._voxels[ID] for ID in uniqs])
-        # add map occupancies with collected voxel counts of points to be added
-        occupancies += counts
-
-        # split into two groups, those that were changed and those that require
-        # removal
-
-        # delete any less than 1
-        inds = occupancies < self.occupancy_threshold
-        removed_ids = uniqs[inds]
-        changed_ids = uniqs[np.logical_not(inds)]
-        changed_occupancies = occupancies[np.logical_not(inds)]
-        for ID in removed_ids:
-            del self._voxels[ID]
-
-        # set those with occupancy 1 or more 
-        # set occupancies of voxels with their new values
-        for i, ID in enumerate(changed_ids):
-            self._voxels[ID] = changed_occupancies[i]
-
-        #for ID in increment_inds:
-        #    if ID not in self._voxels:
-        #        new_ids.append(ID)
-        #    update_func(ID, increment)
-        #    #self._update_voxel_func_user(ID, increments[i], userdata[i])
-
-        #self.removed_ids = np.asarray(self.removed_ids)
-        #new_ids = np.asarray(new_ids)
-        if self._use_bloom:
-            self.bloomfilter.add_voxel_ids(changed_ids)
-            self.bloomfilter.remove_voxel_ids(removed_ids) 
-
-    # TODO do add_points and removepoints properly by apportioning the point 
-    # probability amongst surrounding voxels, like kernel density estimation.
-    # This update should also be more sophisticated in terms of the sensor model.
-    # TODO specify sigma as a covariance matrix for each point to be added
-    # (this takes into account the pose of the sensor)
-    def add_points(self, xyzs, pose=None, userdata=None, increment=1, return_modified=False, sigma=0.5):
-        """ convert points to voxel indices and append them to this occupied 
-        voxel list """
-        # TODO should really take a point cloud object rather than xyzs
-        if len(xyzs) == 0:
-            return
-        xyzs = np.atleast_2d(xyzs) # for single points
-        assert xyzs.shape[1] == 3, 'Points should be in a n by 3 array format'
-        if pose is not None:
-            xyzs = pose.transformPoints(xyzs)
-
-        inds = pointstovoxels(xyzs, resolution=self.resolution, latticetype=self.lattice)
-        # TODO check that voxel indices are not out of bounds
-
-        if self.use_KDE:
-            # TODO is similar to mgrid used in dilate, refactor these together or 
-            # remove dilate as it is not needed now we are doing kernel density 
-            # estimation
-            surrounding_inds = np.int16((np.indices((3,3,3)) - 1).reshape(3, -1).T)
-
-            # Add voxels to be incremented according to kernel density estimation
-            increment_inds = []
-            increments = []
-            # TODO rather than use a list preallocate the array at 27 * len(xyzs)?
-            # TODO make this work for single points
-            for xyz, ind in zip(xyzs, inds):
-                #increments.extend(np.int8(increment*self.KDE(xyz/self.resolution, ind)).ravel())
-                kde = self.KDE(xyz/self.resolution, ind, sigma=sigma)
-                #increments.extend(np.int8(increment*kde).ravel())
-                increments.extend((increment*kde).ravel())
-                # Now add these increments to the appropriate indices
-                increment_inds.extend(ind + surrounding_inds)
-
-            increment_inds = np.asarray(increment_inds)
-            increments = np.asarray(increments)
-
-            # remove increments that are too small to consider
-            bigenough_inds = np.abs(increments) >= self.occupancy_threshold
-            increments = increments[bigenough_inds]
-            increment_inds = increment_inds[bigenough_inds]
-        else:
-            increment_inds = inds
-
-        self._update_voxels(increment_inds, increment, userdata)
-
-        assert len(self._voxels) < self.maxvoxels, 'Max voxels exceeded for bloom table'
-        return increment_inds
-
-    def removepoints(self, xyzs, decrement=1, return_removed=False, **args):
-        '''Decrement the observations in the voxels that the xyzs fall 
-        into.'''
-        return self.add_points(xyzs, pose=None, increment=-decrement, return_modified=return_removed, **args)
-
-        #If the delete flag is set, then remove the voxels that 
-        #xyzs fall into. Return both the voxels that xyzs fall into and the 
-        #total decrement amount for that voxel.
-
-    def savexyz(self, fname):
-        """ saves the finest occupied list to fname in x y z format """
-        np.savetxt(fname, self.getpoints())
-
-    def calccollisions(self, pose, xyzs, returntotal=True, query=False):
-        '''The intersection of points with this occupied voxel list.
-        If query then  return the density for each point after transformation 
-        by the given pose, rather than just a count of hits.  
-        '''
-        # reuse transformed voxels to save initialising the fourth column it to
-        # zero each time.  This is some nasty code I hope the speed increase is
-        # worth it.
-        if self._transformedvoxels is None or len(self._transformedvoxels) != len(xyzs):
-            self._transformedvoxels = pointstovoxels(xyzs, resolution=self.resolution, pose=pose, out=None)
-        else:
-            self._transformedvoxels = pointstovoxels(xyzs, resolution=self.resolution, pose=pose, out=self._transformedvoxels)
-
-        _pack(self._transformedvoxels)
-
-        if query:
-            return [self._voxels[ID] for ID in self._transformedvoxels]
-            #return sum(self._voxels[ID] for ID in self._transformedvoxels)
-
-        if self._use_bloom:
-            present = self.bloomfilter.contains(self._transformedvoxels)
-            if returntotal:
-                return np.sum(present)
-            else:
-                return present
-        else:
-            if returntotal:
-                return sum(ID in self._voxels for ID in self._transformedvoxels)
-            else:
-                return [ID in self._voxels for ID in self._transformedvoxels]
-
-    def cost_func(self, pose, xyzs):
-        return -self.calccollisions(pose, xyzs)
-
-    def segment_ovl(self, pose, xyzs):
-        ''' returns the inliers and outliers of the intersection of a point 
-        cloud with this occupied voxel list.'''
-        xyzs = poseutil.transformPoints(xyzs, pose)[0]
-        overlap_inds = np.array(self.calccollisions(None, xyzs, returntotal=False))
-        inliers = xyzs[overlap_inds]
-        outliers = xyzs[np.logical_not(overlap_inds)]
-        return inliers, outliers
-
-if __name__ == "__main__":
-    import doctest
-    doctest.testmod()

=== added file 'mrol_mapping/optimization.py'
--- mrol_mapping/optimization.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/optimization.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,173 @@
+#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
+#
+#This program is distributed in the hope that it will be useful,
+#but WITHOUT ANY WARRANTY; without even the implied warranty of
+#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#GNU Lesser General Public License for more details.
+#
+#You should have received a copy of the GNU Lesser General Public License
+#along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+from __future__ import division
+import time
+import numpy as np
+import poseutil
+
+# TODO this function seems to be unused
+def _cost_min_scipy(objective_func, initialpose, args):
+    import scipy.optimize as optimize
+    initialpose = initialpose.getTuple()
+    returns = optimize.fmin(objective_func, initialpose, args=args, disp=True, full_output=True)
+    #returns = optimize.fmin_powell(objective_func, initialpose, args=args, disp=True, full_output=True)
+    #returns = optimize.fmin_slsqp(objective_func, initialpose, args=args, full_output=True) # doesn't work?    
+    #initpose = np.asarray(initialpose)
+    #delta = np.asarray((0.5, 0.5, 0.5, 0.2, 0.2, 0.2))
+    #lower = initpose - delta
+    #upper = initpose + delta    
+    #returns = optimize.anneal(objective_func, initialpose, args=args, lower=lower, upper=upper)
+    bestpose = poseutil.Pose3D(X=returns[0])
+    cost = returns[1]
+    return bestpose, cost
+
+
+def _get_offsets():
+    # TODO Try parallel testing of these poses.
+    I3 = np.identity(3)
+    X = np.array([1,0,0])
+    Y = np.array([0,1,0])
+    Z = np.array([0,0,1])
+    offsets = np.vstack((np.identity(6), -np.identity(6)))
+    x_ = np.hstack([np.vstack([X,X,X]),I3])
+    x__ = np.hstack([np.vstack([X,X,X]),-I3])
+    y_ = np.hstack([np.vstack([Y,Y,Y]),I3])
+    y__ = np.hstack([np.vstack([Y,Y,Y]),-I3])
+    z_ = np.hstack([np.vstack([Z,Z,Z]),I3])
+    z__ = np.hstack([np.vstack([Z,Z,Z]),-I3])
+    offsets = np.vstack([offsets,x_,-x_,x__,-x__,y_,-y_,y__,-y__,z_,-z_,z__,-z__])
+    offsets = np.int32(offsets)
+
+    #offsets = np.vstack((np.identity(6), -np.identity(6)))
+    return offsets
+
+def cost_min(cost_func, initialpose, args, dx, dq, max_iterations=100, verbosity=1):
+    ''' The format of this function allows standard optimizers from the
+    scipy.optimize library and arbitrary cost functions.'''
+    dpose = np.array((dx,dx,dx,dq,dq,dq))
+    already_checked = {}
+
+    # TODO this step approach is only correct for small dq
+
+    offsets = _get_offsets()
+    initialoverlap = -cost_func(initialpose, args[0])
+    previousmax = initialoverlap
+    maxo = initialoverlap
+    beststep = (0,0,0,0,0,0)
+    already_checked[beststep] = initialoverlap
+    called_count = 1
+    cost_func_time = 0
+    if verbosity > 1: print 'dx, dq: ', dx, np.degrees(dq), 'degrees'
+    for iter_count in range(max_iterations):
+        steps = beststep + offsets
+        overlaps = []
+        # check poses surrounding beststep
+        for step in steps:
+            step_tup = tuple(step)
+            # optimization to save calculating the cost for poses which we have 
+            # previously calculated the cost for
+            # TODO this is dodgy because the poses consist of floats
+            if step_tup in already_checked: 
+                overlaps.append(already_checked[step_tup])
+                continue
+            else:
+                start = time.time()
+                pose = initialpose.getTuple() + step * dpose 
+                pose = poseutil.Pose3D(pose)
+                cost = -cost_func(pose, args[0]) # negative because maximiser
+                cost_func_time += time.time() - start
+                already_checked[step_tup] = cost
+                called_count += 1
+                overlaps.append(cost)
+                if verbosity > 3:
+                    print pose, cost
+        overlaps = np.array(overlaps)
+        max_overlap_ind = np.argmax(overlaps)
+        maxo = overlaps[max_overlap_ind]
+        if sum(overlaps == maxo) > 1:
+            print 'WARNING: multiple maxima'
+        if verbosity > 2:
+            print iter_count, ':', overlaps, maxo
+        # break if current pose is maximum in the case when alternative pose is
+        # of equal overlap pick the previous pose
+        if maxo <= previousmax: 
+            maxo = previousmax
+            break
+        else:
+            # re-assign for next loop
+            beststep = steps[max_overlap_ind]
+            previousmax = maxo
+        if verbosity > 1:
+            print 'Best step:', beststep, maxo
+    if iter_count >= max_iterations:
+        print "WARNING: Maximum number of iterations reached. Solution did not reach convergence."
+    
+    if verbosity > 0:
+        print 'cost function evaluated:', called_count, 'times over', iter_count, 'iterations'
+        print 'Average cost function evaluation time (ms): %.2f' % (1e3 * cost_func_time/called_count)
+        print len(args[0]), 'cost increase:', initialoverlap, '->', maxo
+
+    bestpose = initialpose.getTuple() + beststep * dpose
+    bestpose = poseutil.Pose3D(bestpose)
+    # -ve again because it is standard for the calling function to assume a
+    # minimisation.
+    return bestpose, maxo
+   
+def plotobjective(cost_func, initialpose, xyzs, plot_range=(-0.2, 0.2, np.radians(-5), np.radians(5)), dx=None, dq=None):
+    '''
+    Plots the cost function for various poses, centered about the initialpose.
+    '''
+    dofs = {0:'x', 1:'y', 2:'z', 3:'rotx', 4:'roty', 5:'rotz'}
+    xmin, xmax = plot_range[:2]
+    qmin, qmax = plot_range[2:]
+    if dx == None:
+        dx = (xmax-xmin)/100.0
+    if dq == None:
+        dq = (qmax-qmin)/100.0
+    ranges = np.array([
+           [xmin, xmin, xmin, qmin, qmin, qmin],
+           [xmax, xmax, xmax, qmax, qmax, qmax],
+           [dx, dx, dx, dq, dq, dq]])
+    ranges = ranges.T
+    import pylab
+    pylab.ion()
+    pylab.figure()
+    pylab.subplot(2, 1, 1)
+    for i in range(3):
+        X = np.arange(ranges[i, 0], ranges[i, 1], ranges[i, 2]) + initialpose[i]
+        pose = np.array(initialpose, dtype=float)
+        Y = []
+        for x in X:
+            pose[i] = x
+            Y.append(cost_func(pose, xyzs))
+        pylab.plot(X - initialpose[i], Y, 'x-', label=str(dofs[i]))
+        print "+"
+    pylab.legend()
+    pylab.xlabel('m')
+    pylab.ylabel('objective function value')
+    pylab.subplot(2, 1, 2)
+    for i in range(3, 6):
+        X = np.arange(ranges[i, 0], ranges[i, 1], ranges[i, 2]) + initialpose[i]
+        pose = np.array(initialpose, dtype=float)
+        Y = []
+        for x in X:
+            pose[i] = x
+            Y.append(cost_func(pose, xyzs))
+        pylab.plot(X - initialpose[i], Y, 'x-', label=str(dofs[i]))
+        print "*"
+    pylab.legend()
+    pylab.xlabel('rad')
+    pylab.ylabel('objective function value')
+    #pylab.show()
+    #pylab.draw()
+
+
+

=== removed file 'mrol_mapping/optimization.py'
--- mrol_mapping/optimization.py	2012-03-04 17:10:07 +0000
+++ mrol_mapping/optimization.py	1970-01-01 00:00:00 +0000
@@ -1,173 +0,0 @@
-#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
-#
-#This program is distributed in the hope that it will be useful,
-#but WITHOUT ANY WARRANTY; without even the implied warranty of
-#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#GNU Lesser General Public License for more details.
-#
-#You should have received a copy of the GNU Lesser General Public License
-#along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-from __future__ import division
-import time
-import numpy as np
-import poseutil
-
-# TODO this function seems to be unused
-def _cost_min_scipy(objective_func, initialpose, args):
-    import scipy.optimize as optimize
-    initialpose = initialpose.getTuple()
-    returns = optimize.fmin(objective_func, initialpose, args=args, disp=True, full_output=True)
-    #returns = optimize.fmin_powell(objective_func, initialpose, args=args, disp=True, full_output=True)
-    #returns = optimize.fmin_slsqp(objective_func, initialpose, args=args, full_output=True) # doesn't work?    
-    #initpose = np.asarray(initialpose)
-    #delta = np.asarray((0.5, 0.5, 0.5, 0.2, 0.2, 0.2))
-    #lower = initpose - delta
-    #upper = initpose + delta    
-    #returns = optimize.anneal(objective_func, initialpose, args=args, lower=lower, upper=upper)
-    bestpose = poseutil.Pose3D(X=returns[0])
-    cost = returns[1]
-    return bestpose, cost
-
-
-def _get_offsets():
-    # TODO Try parallel testing of these poses.
-    I3 = np.identity(3)
-    X = np.array([1,0,0])
-    Y = np.array([0,1,0])
-    Z = np.array([0,0,1])
-    offsets = np.vstack((np.identity(6), -np.identity(6)))
-    x_ = np.hstack([np.vstack([X,X,X]),I3])
-    x__ = np.hstack([np.vstack([X,X,X]),-I3])
-    y_ = np.hstack([np.vstack([Y,Y,Y]),I3])
-    y__ = np.hstack([np.vstack([Y,Y,Y]),-I3])
-    z_ = np.hstack([np.vstack([Z,Z,Z]),I3])
-    z__ = np.hstack([np.vstack([Z,Z,Z]),-I3])
-    offsets = np.vstack([offsets,x_,-x_,x__,-x__,y_,-y_,y__,-y__,z_,-z_,z__,-z__])
-    offsets = np.int32(offsets)
-
-    #offsets = np.vstack((np.identity(6), -np.identity(6)))
-    return offsets
-
-def cost_min(cost_func, initialpose, args, dx, dq, max_iterations=100, verbosity=1):
-    ''' The format of this function allows standard optimizers from the
-    scipy.optimize library and arbitrary cost functions.'''
-    dpose = np.array((dx,dx,dx,dq,dq,dq))
-    already_checked = {}
-
-    # TODO this step approach is only correct for small dq
-
-    offsets = _get_offsets()
-    initialoverlap = -cost_func(initialpose, args[0])
-    previousmax = initialoverlap
-    maxo = initialoverlap
-    beststep = (0,0,0,0,0,0)
-    already_checked[beststep] = initialoverlap
-    called_count = 1
-    cost_func_time = 0
-    if verbosity > 1: print 'dx, dq: ', dx, np.degrees(dq), 'degrees'
-    for iter_count in range(max_iterations):
-        steps = beststep + offsets
-        overlaps = []
-        # check poses surrounding beststep
-        for step in steps:
-            step_tup = tuple(step)
-            # optimization to save calculating the cost for poses which we have 
-            # previously calculated the cost for
-            # TODO this is dodgy because the poses consist of floats
-            if step_tup in already_checked: 
-                overlaps.append(already_checked[step_tup])
-                continue
-            else:
-                start = time.time()
-                pose = initialpose.getTuple() + step * dpose 
-                pose = poseutil.Pose3D(pose)
-                cost = -cost_func(pose, args[0]) # negative because maximiser
-                cost_func_time += time.time() - start
-                already_checked[step_tup] = cost
-                called_count += 1
-                overlaps.append(cost)
-                if verbosity > 3:
-                    print pose, cost
-        overlaps = np.array(overlaps)
-        max_overlap_ind = np.argmax(overlaps)
-        maxo = overlaps[max_overlap_ind]
-        if sum(overlaps == maxo) > 1:
-            print 'WARNING: multiple maxima'
-        if verbosity > 2:
-            print iter_count, ':', overlaps, maxo
-        # break if current pose is maximum in the case when alternative pose is
-        # of equal overlap pick the previous pose
-        if maxo <= previousmax: 
-            maxo = previousmax
-            break
-        else:
-            # re-assign for next loop
-            beststep = steps[max_overlap_ind]
-            previousmax = maxo
-        if verbosity > 1:
-            print 'Best step:', beststep, maxo
-    if iter_count >= max_iterations:
-        print "WARNING: Maximum number of iterations reached. Solution did not reach convergence."
-    
-    if verbosity > 0:
-        print 'cost function evaluated:', called_count, 'times over', iter_count, 'iterations'
-        print 'Average cost function evaluation time (ms): %.2f' % (1e3 * cost_func_time/called_count)
-        print len(args[0]), 'cost increase:', initialoverlap, '->', maxo
-
-    bestpose = initialpose.getTuple() + beststep * dpose
-    bestpose = poseutil.Pose3D(bestpose)
-    # -ve again because it is standard for the calling function to assume a
-    # minimisation.
-    return bestpose, maxo
-   
-def plotobjective(cost_func, initialpose, xyzs, plot_range=(-0.2, 0.2, np.radians(-5), np.radians(5)), dx=None, dq=None):
-    '''
-    Plots the cost function for various poses, centered about the initialpose.
-    '''
-    dofs = {0:'x', 1:'y', 2:'z', 3:'rotx', 4:'roty', 5:'rotz'}
-    xmin, xmax = plot_range[:2]
-    qmin, qmax = plot_range[2:]
-    if dx == None:
-        dx = (xmax-xmin)/100.0
-    if dq == None:
-        dq = (qmax-qmin)/100.0
-    ranges = np.array([
-           [xmin, xmin, xmin, qmin, qmin, qmin],
-           [xmax, xmax, xmax, qmax, qmax, qmax],
-           [dx, dx, dx, dq, dq, dq]])
-    ranges = ranges.T
-    import pylab
-    pylab.ion()
-    pylab.figure()
-    pylab.subplot(2, 1, 1)
-    for i in range(3):
-        X = np.arange(ranges[i, 0], ranges[i, 1], ranges[i, 2]) + initialpose[i]
-        pose = np.array(initialpose, dtype=float)
-        Y = []
-        for x in X:
-            pose[i] = x
-            Y.append(cost_func(pose, xyzs))
-        pylab.plot(X - initialpose[i], Y, 'x-', label=str(dofs[i]))
-        print "+"
-    pylab.legend()
-    pylab.xlabel('m')
-    pylab.ylabel('objective function value')
-    pylab.subplot(2, 1, 2)
-    for i in range(3, 6):
-        X = np.arange(ranges[i, 0], ranges[i, 1], ranges[i, 2]) + initialpose[i]
-        pose = np.array(initialpose, dtype=float)
-        Y = []
-        for x in X:
-            pose[i] = x
-            Y.append(cost_func(pose, xyzs))
-        pylab.plot(X - initialpose[i], Y, 'x-', label=str(dofs[i]))
-        print "*"
-    pylab.legend()
-    pylab.xlabel('rad')
-    pylab.ylabel('objective function value')
-    #pylab.show()
-    #pylab.draw()
-
-
-

=== added file 'mrol_mapping/pointcloud.py'
--- mrol_mapping/pointcloud.py	1970-01-01 00:00:00 +0000
+++ mrol_mapping/pointcloud.py	2012-11-20 22:16:18 +0000
@@ -0,0 +1,229 @@
+'''
+Point cloud module
+Author: Julian Ryde and Nick Hillier
+'''
+#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
+#
+#This program is distributed in the hope that it will be useful,
+#but WITHOUT ANY WARRANTY; without even the implied warranty of
+#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#GNU Lesser General Public License for more details.
+#
+#You should have received a copy of the GNU Lesser General Public License
+#along with this program.  If not, see <http://www.gnu.org/licenses/>.
+
+import numpy as np
+import os
+
+import util
+
+
+def nearest_neighbour(A, B):
+    '''Determine the distances the nearest neighbours in B to the points in A.  
+    Uses a slow but accurate and hopefully bug free method
+    
+    Note that order is important.'''
+    # TODO quicker version of this
+    NN_dists = []
+    NN_inds = []
+    for i, a in enumerate(A):
+        dists = np.sum((B - a) ** 2, axis=1)
+        ind = np.argmin(dists)
+        NN_dists.append(dists[ind])
+        NN_inds.append(ind)
+    return np.asarray(np.sqrt(NN_dists)), np.asarray(NN_inds)
+
+# TODO unit test for this?
+def equal(A, B):
+    '''Is true is the arrays A and B are equal ignoring their row order'''
+    return PointCloud(A) == PointCloud(B)
+
+def load(fname, delimiter=None, cols=(0, 1, 2), headerlines=0):
+    '''Loads a point cloud from disk whether it is a text array, numpy 
+    binary array, assumes that each row is a point and the first three 
+    columns are the coordinates and if specified as a 6 column dataset, the last 3 columns are
+    RGB colour data'''
+    if len(cols) != 3:
+        if len(cols) != 6:
+            print "Data must be 3 or 6 columns"
+            return PointCloud(np.array([]))
+    try:
+        X = np.loadtxt(fname, usecols=cols, delimiter=delimiter, skiprows=headerlines)
+    except:
+        X = np.load(fname)
+    if X.shape[1] != 3:
+        if X.shape[1] != 6:
+            print "Data must be 3 or 6 columns"
+            return PointCloud(np.array([]))
+    return PointCloud(X[:,:X.shape[1]])
+    
+def save(fname, xyzs, text=False):
+    '''Save a points in Nx3 arrays to disk. By default it is saved as a numpy 
+    binary array, but it can be saved as plain text by setting the 
+    'text' flag'''
+    if text:
+        np.savetxt(fname,xyzs)
+    else:
+        np.save(fname,xyzs)
+
+def save_ply(fname, xyzs):
+    ''' Save points in Nx3 arrays as .ply Polygon File Format.  This format is
+    popular and can be loaded by programs such as blender, meshlab and the
+    point cloud library. '''
+
+    header = '''ply
+format ascii 1.0
+element vertex %d
+property float x
+property float y
+property float z
+end_header
+''' % len(xyzs)
+
+    # TODO new version of numpy.savetxt will support header option
+    F = open(fname, 'w')
+    F.write(header)
+    for x, y, z in xyzs:
+        F.write('%.03f %.03f %.03f\n' % (x, y, z))
+    return
+
+class PointCloud:
+    # TODO point clouds should be stored as ints in millimetres?
+
+    def __init__(self, arr, timestamp=0):
+        '''Initialise the point cloud with an array of points'''
+        # TODO handle both n x 3 and 3 by n arrays 
+        # TODO throw error if conversion to an arrays not successful
+        self.RGB = False # set if the point cloud has RGB data associated
+        self.add_points(arr, timestamp)
+        self.pose = None
+        self.userdata = None
+
+    def __eq__(self, pc, tol=0.00001):
+        # TODO pick a proper value for this tol, like np.allclose
+        if len(self) != len(pc):
+            return False
+        xyzs = self.points[:,:3]
+        nn_dists = nearest_neighbour(xyzs, pc.points[:,:3])[0]
+        return np.all(nn_dists < tol)
+        
+    def __sub__(self, pc):
+        if type(pc) == np.ndarray:
+            assert pc.shape[0] == 3, "subtraction operator must be on 3xN arrays, 3x1 lists or pointcloud objects"
+        if type(pc) == list:
+            assert len(pc) == 3, "subtraction operator must be on 3xN arrays, 3x1 lists or pointcloud objects"
+        new_xyzs = self.points[:,:3] - pc
+        return PointCloud(np.hstack((new_xyzs, self.points[:,3:])))
+        #TODO allow subtraction of two pointclouds
+    
+    def __add__(self, pc):
+        if type(pc) == np.ndarray:
+            assert pc.shape[0] == 3, "addition operator must be on 3xN arrays, 3x1 lists or pointcloud objects"
+            new_xyzs = self.points[:,:3] + pc
+            return PointCloud(np.hstack((new_xyzs, self.points[:,3:])))
+        elif type(pc) == list:
+            assert len(pc) == 3, "addition operator must be on 3xN arrays, 3x1 lists or pointcloud objects"
+            new_xyzs = self.points[:,:3] + pc
+            return PointCloud(np.hstack((new_xyzs, self.points[:,3:])))
+        else:
+            # TODO: this requires more thought
+            return self.add_points(pc)
+        
+    def __len__(self):
+        return self.points.shape[0]
+        
+    def save(self, filename):
+        save(filename, self.points)
+
+    def save_ply(self, filename):
+        save_ply(filename, self.points)
+
+    # TODO: pose transform operator, may make addition/subtraction of 3x1 array redundant?
+        
+    def add_points(self, arr, timestamp=0):
+        # TODO enable growing of the pointcloud points
+        arr = np.asarray(arr)
+        if len(arr) > 0:
+            self.points = np.atleast_2d(arr)
+        else:
+            self.points = arr # empty pc object
+        if self.points.shape[1] == 6:
+            self.RGB = True                
+        self.timestamp = timestamp
+
+    def display(self, color=None, title='', scene=None):
+        ''' Displays the point cloud in a visual window.  To display in an
+        existing window supply visual scene object like the one returned by
+        this function. '''
+        import visual # import here only if needed
+        # import here to avoid circular dependency
+        import mrol_mapping.visualiser.dispxyz as dispxyz
+
+        if self.RGB == True:
+            xyzs = self.points[:,:3]
+            if color == None:
+                color = self.points[:,3:6]/np.max(self.points[:,3:6])
+        else:
+            xyzs = self.points
+
+        if scene is None:
+            scene = visual.display(title=title + 'dispxyz')
+
+        #scene.exit = False
+        MT = dispxyz.EnableMouseThread(scene)
+        MT.start()
+        dispxyz.showpts(xyzs, pose=self.pose, color=color)
+        scene.forward = (0,1,0)
+        scene.up = (0,0,1)
+        scene.forward = (0,0,-1)
+        return scene, MT
+
+    # TODO merge other implementation of boxfilter into this
+    def boxfilter(self, xmin=None, ymin=None, zmin=None, xmax=None, ymax=None, zmax=None):
+        '''Remove points outside of specified limits'''
+        for i, limit in enumerate([xmin, ymin, zmin]):
+            if limit is not None:
+                self.points = self.points[self.points[:,i] > limit]
+        for i, limit in enumerate([xmax, ymax, zmax]):
+            if limit is not None:
+                self.points = self.points[self.points[:,i] < limit]
+
+    # TODO move the util.volumetricsample code to here 
+    def volumetricsample(self, res):
+        '''Returns a downsampled point cloud volumetrically according to the 
+        resolution'''
+        xyzs = self.points[:,:3]
+        xyzs, inds = util.volumetricsample(xyzs, res, return_inds=True)
+        #self.points = np.hstack([xyzs, self.points[inds, 3:]])
+        return PointCloud(np.hstack([xyzs, self.points[inds, 3:]]))
+
+class ScansDir: 
+    # TODO deprecated, just use a list of file names and pointcloud load instead
+    ''' Reads in a group of pointcloud files from a directory '''
+    # adapted from icra2011/make_template
+    def __init__(self, basedir, skip_to=None):
+        self.basedir = basedir
+        fnames = os.listdir(basedir)
+        fnames.sort()
+        self.basenames = []
+        skip_flag = False
+        for fname in fnames:
+            # TODO pass in startswith as an argument or make all files saved with this interface start with mrolpc
+            if not fname.startswith('mrolpc'): continue
+            if skip_to:
+                if (not fname.startswith(skip_to)) and (not skip_flag):
+                    continue
+                else:
+                    skip_flag = True
+            self.basenames.append(fname)
+
+    def __iter__(self):
+        return self
+
+    def next(self):
+        '''Returns a scan'''
+        if len(self.basenames) == 0: raise StopIteration
+        self.curname = self.basedir+'/'+self.basenames.pop(0)
+        print self.curname
+        xyzs = load(self.curname)
+        return xyzs

=== removed file 'mrol_mapping/pointcloud.py'
--- mrol_mapping/pointcloud.py	2012-06-25 16:36:58 +0000
+++ mrol_mapping/pointcloud.py	1970-01-01 00:00:00 +0000
@@ -1,227 +0,0 @@
-'''
-Point cloud module
-Author: Julian Ryde and Nick Hillier
-'''
-#Copyright 2010-2011, Julian Ryde and Nicholas Hillier.
-#
-#This program is distributed in the hope that it will be useful,
-#but WITHOUT ANY WARRANTY; without even the implied warranty of
-#MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#GNU Lesser General Public License for more details.
-#
-#You should have received a copy of the GNU Lesser General Public License
-#along with this program.  If not, see <http://www.gnu.org/licenses/>.
-
-import numpy as np
-import os
-
-import util
-
-
-def nearest_neighbour(A, B):
-    '''Determine the distances the nearest neighbours in B to the points in A.  
-    Uses a slow but accurate and hopefully bug free method
-    
-    Note that order is important.'''
-    # TODO quicker version of this
-    NN_dists = []
-    NN_inds = []
-    for i, a in enumerate(A):
-        dists = np.sum((B - a) ** 2, axis=1)
-        ind = np.argmin(dists)
-        NN_dists.append(dists[ind])
-        NN_inds.append(ind)
-    return np.asarray(np.sqrt(NN_dists)), np.asarray(NN_inds)
-
-# TODO unit test for this?
-def equal(A, B):