stereo_ptam
stereo_ptam copied to clipboard
How to save the poses and map points?
How can I access the poses and map points information so that I can save them in a file?
I didn't test this, but something like this should work for the map points. Just paste it and put it before the line sptam.stop()
:
for kf in sptam.graph.keyframes():
for m in kf.measurements():
if m.from_triangulation():
pts.append([m.mappoint.position, m.mappoint.normal, m.mappoint.color])
with open('pointcloud.txt', 'w') as f:
f.write('x,y,z,nx,ny,nz,r,g,b\n')
for pt in pts:
pos, n, rgb = pt
f.write(','.join([str(el) for p in pos]))
f.write(','.join([str(el) for p in n]))
f.write(','.join([str(el) for p in rgb]))
f.write('\n)
You can follow this. Some errors and tempos in codes by @LaurensJN , so I slightly modify it.
`
def stop(self):
self.mapping.stop()
if self.loop_closing is not None:
self.loop_closing.stop()
sptam.tracker.optimizer.save("tracker-optimizer.g2o")
pts = list()
for kf in sptam.graph.keyframes():
for m in kf.measurements():
if m.from_triangulation():
pts.append([m.mappoint.position, m.mappoint.normal, m.mappoint.color])
with open('pointcloud.txt', 'w') as f:
f.write('x,y,z,nx,ny,nz,r,g,b\n')
for pt in pts:
pos, n, rgb = pt
f.write(','.join([str(p) for p in pos]))
f.write(',')
f.write(','.join([str(p) for p in n]))
f.write(',')
f.write(','.join([str(p) for p in rgb]))
f.write('\n')
`