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])