← Back to branch summary

~mrol-dev/mrol/trunk

« back to all changes in this revision

Viewing changes to test/mapper_test.py

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

Show diffs side-by-side

added added

removed removed

Lines of Context:
test/mapper_test.py
9
9
 
10
10
import mrol_mapping.mapper as mapper
11
11
import mrol_mapping.poseutil as poseutil
12
 
import mrol_mapping.mrol as mrol 
13
12
import mrol_mapping.occupiedlist as occupiedlist
14
13
import mrol_mapping.pointcloud as pointcloud
15
14
 
41
40
        poses.append(P)
42
41
    return poses
43
42
 
44
 
true_poses = load_poses(basedir + 'poses.txt')
 
43
# for some reason these poses derived from blender are wrong
 
44
# true_poses = load_poses(basedir + 'poses.txt')
 
45
 
 
46
true_poses_arr = np.loadtxt(basedir + 'poses2.txt')
 
47
true_poses_arr[:, 3:] = np.radians(true_poses_arr[:, 3:])
 
48
true_poses = []
 
49
for P in true_poses_arr:
 
50
    true_poses.append(poseutil.Pose3D(P))
45
51
 
46
52
def get_PC(i):
47
53
    '''Loads the i-th point cloud from test data and attach the true pose to 
92
98
 
93
99
#class TestMapper(unittest.TestCase):
94
100
class TestMapper:
95
 
    
96
101
 
97
102
    def setUp(self):
98
103
        #np.random.seed(6)
105
110
    def test_saveload(self): 
106
111
        print "test_saveload"
107
112
        # TODO use tempfile module?
108
 
        vmap = self.vmap
109
113
        fname = '/tmp/vmap.map'
110
 
        vmap.save(fname)
 
114
        self.vmap.save(fname)
111
115
        loaded = mapper.loadmap(fname)
 
116
 
 
117
        assert len(loaded) == 58467
 
118
        
112
119
        # TODO assert equals needs __eq__ method on VoxelMap
113
120
 
114
121
    def OFFtest_raytrace(self): 
167
174
        # make first guess pose correct to fix trajectory in same coordinate 
168
175
        # system as true poses
169
176
        start = time.time()
170
 
        vmap = mapper.buildmap(iter(pcs[:scans]), resolution=0.03, visualise=visualise)
 
177
        vmap = mapper.buildmap(iter(pcs[:scans]), resolution=0.03, visualise=visualise, odometry_map=False)
171
178
        taken = time.time() - start
172
 
        # check the trajectory is correct
 
179
        # print the trajectory poses
173
180
        print 'Pose error'
174
181
        print '(m)\t(degs)'
175
182
        for i, bestpose in enumerate(vmap.trajectory):
181
188
 
182
189
    def test_slow_mapbuild_6DOF(self): 
183
190
        # longer test building map
184
 
 
185
 
        # For some reason only first few scans are working at the moment
186
191
        vmap = self._map_build(10) # 16 scans available
187
192
 
188
193
    def OFFtest_mapper_global_localisation(self):
226
231
 
227
232
def _run_mapping():
228
233
    tm = TestMapper()
229
 
    vmap = tm._map_build(10)
 
234
    vmap = tm._map_build(16)
230
235
 
231
236
if __name__ == '__main__':
232
237
    # run for profiling