fit_plane.py
Go to the documentation of this file.
1 #!/usr/bin/python
2 
3 """
4 Fit a plane to the fiducials in the map as a quantitative test of the
5 map quality. Assumes they are all on a ceiling
6 """
7 
8 import matplotlib.pyplot as plt
9 from mpl_toolkits.mplot3d import Axes3D
10 import numpy as np
11 import os
12 import math
13 import sys
14 import argparse
15 from standard_fit import standard_fit, distance, projection
16 
17 def closest_angle(old, new):
18  angle = new
19  dif = angle - old
20  if dif > 180:
21  dif -= 360
22  elif dif < -180:
23  dif += 360
24  if abs(dif) > 90:
25  angle += 180
26  if angle > 180:
27  angle -= 360
28  elif angle < -180:
29  angle += 360
30  return angle
31 
32 # parse args
33 map_file = os.environ["HOME"] + "/.ros/slam/map.txt"
34 parser = argparse.ArgumentParser()
35 parser.add_argument("--map_file", help="Name of map file ", default=map_file, type=str)
36 parser.add_argument("--adjust", help="Adjust points to fit plane", action="store_true")
37 args = parser.parse_args()
38 
39 # read data from map
40 ids = []
41 points = []
42 roll = []
43 pitch = []
44 other = []
45 
46 adjust = args.adjust
47 
48 with open(args.map_file) as file:
49  lines = file.readlines()
50  for line in lines:
51  parts = line.split()
52  ids.append(int(parts[0]))
53  points.append([float(parts[1]), float(parts[2]), float(parts[3])])
54  roll.append(float(parts[4]))
55  pitch.append(float(parts[5]))
56  other.append(" ".join(parts[6:]))
57 
58 # plot raw data
59 plt.figure()
60 
61 points = np.array(points)
62 ax = plt.subplot(111, projection='3d')
63 xs = points[:,0]
64 ys = points[:,1]
65 zs = points[:,2]
66 
67 ax.scatter(xs, ys, zs, color='b')
68 
69 C, N = standard_fit(points)
70 print("Plane normal: %s" % N)
71 
72 # plot points projected onto plane
73 fixed_points = projection(points, C, N)
74 xs = fixed_points[:,0]
75 ys = fixed_points[:,1]
76 zs = fixed_points[:,2]
77 ax.scatter(xs, ys, zs, color='r')
78 
79 errors = distance(points, C, N)
80 residual = np.linalg.norm(errors)
81 
82 slopex = math.atan2(N[0], N[2]) * 180.0 / math.pi
83 slopey = math.atan2(N[1], N[2]) * 180.0 / math.pi
84 print("slope: %f deg in X %f deg in Y" % (slopex, slopey))
85 
86 print("residual: %f" % residual)
87 
88 # plot plane
89 xlim = ax.get_xlim()
90 ylim = ax.get_ylim()
91 zlim = ax.get_zlim()
92 X,Y = np.meshgrid(np.arange(xlim[0], xlim[1]),
93  np.arange(ylim[0], ylim[1]))
94 D = -C.dot(N)
95 Z = (-N[0] * X - N[1] * Y - D) / N[2]
96 ax.plot_wireframe(X,Y,Z, color='k')
97 
98 ax.set_xlabel('x')
99 ax.set_ylabel('y')
100 ax.set_zlabel('z')
101 plt.show()
102 
103 
104 if adjust:
105  print("Saving adjusted map")
106  os.rename(map_file, map_file + ".bak")
107  with open(map_file, 'w') as file:
108  for i in range(len(errors)):
109  file.write("%d %f %f %f %f %f %s\n" % (ids[i], xs[i], ys[i], zs[i],
110  closest_angle(roll[i], slopex),
111  closest_angle(pitch[i], slopey),
112  other[i]))
def closest_angle(old, new)
Definition: fit_plane.py:17
def projection(x, C, N)
Definition: standard_fit.py:55


fiducial_slam
Author(s): Jim Vaughan
autogenerated on Tue Jun 1 2021 03:03:29