def _calibrate(self):
self.has_image = False
self.image_capture.stream = True
# Save point clouds
for i in xrange(2):
save_point_cloud('PC' + str(i) + '.ply', self._point_cloud[i])
self.distance = [None, None]
self.normal = [None, None]
self.std = [None, None]
# Compute planes
for i in xrange(2):
if self._is_calibrating:
plane = compute_plane(i, self._point_cloud[i])
self.distance[i], self.normal[i], self.std[i] = plane
if self._is_calibrating:
if self.std[0] < 1.0 and self.std[1] < 1.0 and \
self.normal[0] is not None and self.normal[1] is not None:
response = (True,
((self.distance[0], self.normal[0], self.std[0]),
(self.distance[1], self.normal[1], self.std[1])))
else:
response = (False, LaserTriangulationError())
else:
response = (False, CalibrationCancel())
self._is_calibrating = False
self.image = None
return response
def check_lasers(self):
image = self.image_capture.capture_pattern()
corners = self.image_detection.detect_corners(image)
self.image_capture.flush_laser()
for i in xrange(2):
if not self._is_calibrating:
raise CalibrationCancel()
image = self.image_capture.capture_laser(i)
image = self.image_detection.pattern_mask(image, corners)
lines = self.laser_segmentation.compute_hough_lines(image)
if lines is None:
raise LaserNotDetected()
def check_pattern_and_motor(self):
scan_step = 30
patterns_detected = {}
patterns_sorted = {}
if self._progress_callback is not None:
self._progress_callback(0)
# Capture data
for i in range(0, 360, scan_step):
if not self._is_calibrating:
raise CalibrationCancel()
image = self.image_capture.capture_pattern()
pose = self.image_detection.detect_pose(image)
if pose is not None:
self.image = self.image_detection.draw_pattern(image, pose[2])
patterns_detected[i] = pose[0].T[2][0]
else:
self.image = self.image_detection.detect_pattern(image)
if self._progress_callback is not None:
self._progress_callback(i / 3.6)
self.driver.board.motor_move(scan_step)
# Check pattern detection
if len(patterns_detected) == 0:
raise PatternNotDetected()
# Check motor direction
max_x = max(patterns_detected.values())
max_i = [
key for key, value in list(patterns_detected.items())
if value == max_x
][0]
min_v = max_x
for i in range(max_i, max_i + 360, scan_step):
if i % 360 in patterns_detected:
v = patterns_detected[i % 360]
patterns_sorted[i] = v
if v <= min_v:
min_v = v
else:
raise WrongMotorDirection()
# Move to nearest position
x = np.array(list(patterns_sorted.keys()))
y = np.array(list(patterns_sorted.values()))
A = np.vstack([x, np.ones(len(x))]).T
m, c = np.linalg.lstsq(A, y)[0]
pos = -c / m % 360
if pos > 180:
pos = pos - 360
self.driver.board.motor_move(pos)
def _calibrate(self):
self.has_image = False
self.image_capture.stream = True
self.t = None
self.x = np.array(self.x)
self.y = np.array(self.y)
self.z = np.array(self.z)
points = zip(self.x, self.y, self.z)
if len(points) > 4:
# Fitting a plane
point, normal = fit_plane(points)
if normal[1] > 0:
normal = -normal
# Fitting a circle inside the plane
center, self.R, circle = fit_circle(point, normal, points)
# Get real origin
self.t = center - self.pattern.origin_distance * np.array(normal)
logger.info("Platform calibration ")
logger.info(" Translation: " + str(self.t))
logger.info(" Rotation: " + str(self.R).replace('\n', ''))
logger.info(" Normal: " + str(normal))
print("self.t-------------------------->")
print(self.t)
print("estimated.t-------------------------->")
print(estimated_t)
if self._is_calibrating and self.t is not None and \
np.linalg.norm(self.t - estimated_t) < 100:
response = (True, (self.R, self.t, center, point, normal,
[self.x, self.y, self.z], circle))
else:
if self._is_calibrating:
response = (False, PlatformExtrinsicsError())
else:
response = (False, CalibrationCancel())
self._is_calibrating = False
self.image = None
return response
def _calibrate(self):
self.has_image = False
self.image_capture.stream = True
# Laser triangulation
# Save point clouds
for i in xrange(2):
laser_triangulation.save_point_cloud('PC' + str(i) + '.ply', self._point_cloud[i])
self.distance = [None, None]
self.normal = [None, None]
self.std = [None, None]
# Compute planes
for i in xrange(2):
if self._is_calibrating:
plane = laser_triangulation.compute_plane(i, self._point_cloud[i])
self.distance[i], self.normal[i], self.std[i] = plane
# Platform extrinsics
self.t = None
self.x = np.array(self.x)
self.y = np.array(self.y)
self.z = np.array(self.z)
points = zip(self.x, self.y, self.z)
if len(points) > 4:
# Fitting a plane
point, normal = platform_extrinsics.fit_plane(points)
if normal[1] > 0:
normal = -normal
# Fitting a circle inside the plane
center, self.R, circle = platform_extrinsics.fit_circle(point, normal, points)
# Get real origin
self.t = center - self.pattern.origin_distance * np.array(normal)
logger.info("Platform calibration ")
logger.info(" Translation: " + str(self.t))
logger.info(" Rotation: " + str(self.R).replace('\n', ''))
logger.info(" Normal: " + str(normal))
# Return response
result = True
if self._is_calibrating:
if self.t is not None and \
np.linalg.norm(self.t - platform_extrinsics.estimated_t) < 100:
response_platform_extrinsics = (
self.R, self.t, center, point, normal, [self.x, self.y, self.z], circle)
else:
result = False
if self.std[0] < 1.0 and self.std[1] < 1.0 and \
self.normal[0] is not None and self.normal[1] is not None:
response_laser_triangulation = ((self.distance[0], self.normal[0], self.std[0]),
(self.distance[1], self.normal[1], self.std[1]))
else:
result = False
if result:
response = (True, (response_platform_extrinsics, response_laser_triangulation))
else:
response = (False, ComboCalibrationError())
else:
response = (False, CalibrationCancel())
self._is_calibrating = False
self.image = None
return response