cylinders = get_cylinders_from_scan(logfile.scan_data[i], depth_jump,
minimum_valid_distance, cylinder_offset)
fs.correct(cylinders)
# Output particles.
print_particles(fs.particles, f)
# Output state estimated from all particles.
mean = get_mean(fs.particles)
print >> f, "F %.0f %.0f %.3f" %\
(mean[0] + scanner_displacement * cos(mean[2]),
mean[1] + scanner_displacement * sin(mean[2]),
mean[2])
# Output error ellipse and standard deviation of heading.
errors = get_error_ellipse_and_heading_variance(fs.particles, mean)
print >> f, "E %.3f %.0f %.0f %.3f" % errors
# Output landmarks of particle which is closest to the mean position.
output_particle = min([
(np.linalg.norm(mean[0:2] - fs.particles[i].pose[0:2]),i)
for i in xrange(len(fs.particles)) ])[1]
# Write estimates of landmarks.
write_cylinders(f, "W C",
fs.particles[output_particle].landmark_positions)
# Write covariance matrices.
write_error_ellipses(f, "W E",
fs.particles[output_particle].landmark_covariances)
f.close()
cylinder_offset)
fs.correct(cylinders)
# Output particles.
print_particles(fs.particles, f)
# Output state estimated from all particles.
mean = get_mean(fs.particles)
print >> f, "F %.0f %.0f %.3f" %\
(mean[0] + scanner_displacement * cos(mean[2]),
mean[1] + scanner_displacement * sin(mean[2]),
mean[2])
# Output error ellipse and standard deviation of heading.
errors = get_error_ellipse_and_heading_variance(fs.particles, mean)
print >> f, "E %.3f %.0f %.0f %.3f" % errors
# Output landmarks of particle which is closest to the mean position.
output_particle = min([
(np.linalg.norm(mean[0:2] - fs.particles[i].pose[0:2]), i)
for i in xrange(len(fs.particles))
])[1]
# Write estimates of landmarks.
write_cylinders(f, "W C",
fs.particles[output_particle].landmark_positions)
# Write covariance matrices.
write_error_ellipses(
f, "W E", fs.particles[output_particle].landmark_covariances)
f.close()
cylinders,
points_per_sector=33,
min_distance=300,
inline_threshold=45,
attempts=10,
valid_threshold=0.8)
ransac.try_merge_sectors()
# scans_without_landmarks = get_scans_without_landmarks(
# logfile.scan_data[i], cylinders)
# print(scans_without_landmarks)
# write_cylinders(f_sub_scans, "SC", [(obs[0], obs[1])
# for obs in ransac.landmarks])
cylinders += ransac.landmarks
write_cylinders(f, "D C",
[(obs[1][0], obs[1][1]) for obs in cylinders])
write_walls(f, "D W", [(W[0][0], W[1][0], W[0][1], W[1][1])
for W in ransac.walls])
fs.correct(cylinders)
# Output particles.
# print_particles(fs.particles, f)
# Output state estimated from all particles.
mean = get_mean(fs.particles)
print >> f, "F %.0f %.0f %.3f" %\
(mean[0] + scanner_displacement * cos(mean[2]),
mean[1] + scanner_displacement * sin(mean[2]),
mean[2])
