Merge branch 'devel' of https://github.com/commaai/openpilot into devel-en

selfdrive/locationd/calibrationd.py

@@ -10,19 +10,23 @@ from selfdrive.swaglog import cloudlog
 from common.params import Params, put_nonblocking
 from common.transformations.model import model_height
 from common.transformations.camera import view_frame_from_device_frame, get_view_frame_from_road_frame, \
-                                          eon_intrinsics, get_calib_from_vp, H, W
+                                          get_calib_from_vp, H, W, FOCAL
 
 MPH_TO_MS = 0.44704
 MIN_SPEED_FILTER = 15 * MPH_TO_MS
+MAX_SPEED_STD = 1.5
 MAX_YAW_RATE_FILTER = np.radians(2)  # per second
-INPUTS_NEEDED = 300   # allow to update VP every so many frames
-INPUTS_WANTED = 600   # We want a little bit more than we need for stability
-WRITE_CYCLES = 400  # write every 400 cycles
+
+# This is all 20Hz, blocks needed for efficiency
+BLOCK_SIZE = 100
+INPUTS_NEEDED = 5   # allow to update VP every so many frames
+INPUTS_WANTED = 20   # We want a little bit more than we need for stability
+WRITE_CYCLES = 10  # write every 1000 cycles
 VP_INIT = np.array([W/2., H/2.])
 
 # These validity corners were chosen by looking at 1000
 # and taking most extreme cases with some margin.
-VP_VALIDITY_CORNERS = np.array([[W//2 - 150, 280], [W//2 + 150, 540]])
+VP_VALIDITY_CORNERS = np.array([[W//2 - 120, 300], [W//2 + 120, 520]])
 DEBUG = os.getenv("DEBUG") is not None
 
 
@@ -31,13 +35,29 @@ def is_calibration_valid(vp):
          vp[1] > VP_VALIDITY_CORNERS[0,1] and vp[1] < VP_VALIDITY_CORNERS[1,1]
 
 
+def sanity_clip(vp):
+  if np.isnan(vp).any():
+    vp = VP_INIT
+  return [np.clip(vp[0], VP_VALIDITY_CORNERS[0,0] - 20, VP_VALIDITY_CORNERS[1,0] + 20),
+          np.clip(vp[1], VP_VALIDITY_CORNERS[0,1] - 20, VP_VALIDITY_CORNERS[1,1] + 20)]
+
+
+def intrinsics_from_vp(vp):
+  return np.array([
+    [FOCAL,   0.,   vp[0]],
+    [  0.,  FOCAL,  vp[1]],
+    [  0.,    0.,     1.]])
+
+
 class Calibrator():
   def __init__(self, param_put=False):
     self.param_put = param_put
     self.vp = copy.copy(VP_INIT)
-    self.vps = []
+    self.vps = np.zeros((INPUTS_WANTED, 2))
+    self.idx = 0
+    self.block_idx = 0
+    self.valid_blocks = 0
     self.cal_status = Calibration.UNCALIBRATED
-    self.write_counter = 0
     self.just_calibrated = False
 
     # Read calibration
@@ -46,14 +66,19 @@ class Calibrator():
       try:
         calibration_params = json.loads(calibration_params)
         self.vp = np.array(calibration_params["vanishing_point"])
-        self.vps = np.tile(self.vp, (calibration_params['valid_points'], 1)).tolist()
+        if not np.isfinite(self.vp).all():
+          self.vp = copy.copy(VP_INIT)
+        self.vps = np.tile(self.vp, (INPUTS_WANTED, 1))
+        self.valid_blocks = calibration_params['valid_blocks']
+        if not np.isfinite(self.valid_blocks) or self.valid_blocks < 0:
+          self.valid_blocks = 0
         self.update_status()
       except Exception:
         cloudlog.exception("CalibrationParams file found but error encountered")
 
   def update_status(self):
     start_status = self.cal_status
-    if len(self.vps) < INPUTS_NEEDED:
+    if self.valid_blocks < INPUTS_NEEDED:
       self.cal_status = Calibration.UNCALIBRATED
     else:
       self.cal_status = Calibration.CALIBRATED if is_calibration_valid(self.vp) else Calibration.INVALID
@@ -63,19 +88,28 @@ class Calibrator():
     if start_status == Calibration.UNCALIBRATED and end_status == Calibration.CALIBRATED:
       self.just_calibrated = True
 
-  def handle_cam_odom(self, log):
-    trans, rot = log.trans, log.rot
-    if np.linalg.norm(trans) > MIN_SPEED_FILTER and abs(rot[2]) < MAX_YAW_RATE_FILTER:
-      new_vp = eon_intrinsics.dot(view_frame_from_device_frame.dot(trans))
+  def handle_cam_odom(self, trans, rot, trans_std, rot_std):
+    if ((trans[0] > MIN_SPEED_FILTER) and
+        (trans_std[0] < MAX_SPEED_STD) and
+         (abs(rot[2]) < MAX_YAW_RATE_FILTER)):
+      # intrinsics are not eon intrinsics, since this is calibrated frame
+      intrinsics = intrinsics_from_vp(self.vp)
+      new_vp = intrinsics.dot(view_frame_from_device_frame.dot(trans))
       new_vp = new_vp[:2]/new_vp[2]
-      self.vps.append(new_vp)
-      self.vps = self.vps[-INPUTS_WANTED:]
-      self.vp = np.mean(self.vps, axis=0)
+
+      self.vps[self.block_idx] = (self.idx*self.vps[self.block_idx] + (BLOCK_SIZE - self.idx) * new_vp) / float(BLOCK_SIZE)
+      self.idx = (self.idx + 1) % BLOCK_SIZE
+      if self.idx == 0:
+        self.block_idx += 1
+        self.valid_blocks = max(self.block_idx, self.valid_blocks)
+        self.block_idx = self.block_idx % INPUTS_WANTED
+      raw_vp = np.mean(self.vps[:max(1, self.valid_blocks)], axis=0)
+      self.vp = sanity_clip(raw_vp)
       self.update_status()
-      self.write_counter += 1
-      if self.param_put and (self.write_counter % WRITE_CYCLES == 0 or self.just_calibrated):
+
+      if self.param_put and ((self.idx == 0 and self.block_idx == 0) or self.just_calibrated):
         cal_params = {"vanishing_point": list(self.vp),
-                      "valid_points": len(self.vps)}
+                      "valid_blocks": self.valid_blocks}
         put_nonblocking("CalibrationParams", json.dumps(cal_params).encode('utf8'))
       return new_vp
     else:
@@ -88,7 +122,7 @@ class Calibrator():
     cal_send = messaging.new_message()
     cal_send.init('liveCalibration')
     cal_send.liveCalibration.calStatus = self.cal_status
-    cal_send.liveCalibration.calPerc = min(len(self.vps) * 100 // INPUTS_NEEDED, 100)
+    cal_send.liveCalibration.calPerc = min(100 * (self.valid_blocks * BLOCK_SIZE + self.idx) // (INPUTS_NEEDED * BLOCK_SIZE), 100)
     cal_send.liveCalibration.extrinsicMatrix = [float(x) for x in extrinsic_matrix.flatten()]
     cal_send.liveCalibration.rpyCalib = [float(x) for x in calib]
 
@@ -104,15 +138,22 @@ def calibrationd_thread(sm=None, pm=None):
 
   calibrator = Calibrator(param_put=True)
 
-  # buffer with all the messages that still need to be input into the kalman
+  send_counter = 0
   while 1:
     sm.update()
 
-    new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'])
+    if sm.updated['cameraOdometry']:
+      new_vp = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
+                                          sm['cameraOdometry'].rot,
+                                          sm['cameraOdometry'].transStd,
+                                          sm['cameraOdometry'].rotStd)
     if DEBUG and new_vp is not None:
       print('got new vp', new_vp)
 
-    calibrator.send_data(pm)
+    # decimate outputs for efficiency
+    if (send_counter % 5) == 0:
+      calibrator.send_data(pm)
+    send_counter += 1
 
 
 def main(sm=None, pm=None):

selfdrive/locationd/locationd_yawrate.cc

@@ -40,7 +40,7 @@ void Localizer::handle_sensor_events(capnp::List<cereal::SensorEventData>::Reade
 }
 
 void Localizer::handle_camera_odometry(cereal::CameraOdometry::Reader camera_odometry, double current_time) {
-  double R = 100.0 * pow(camera_odometry.getRotStd()[2], 2);
+  double R = pow(30.0 *camera_odometry.getRotStd()[2], 2);
   double meas = camera_odometry.getRot()[2];
   update_state(C_posenet, R, current_time, meas);
 
@@ -73,7 +73,7 @@ Localizer::Localizer() {
     0, 0, 0, 0,
     0, pow(0.1, 2.0), 0, 0,
     0, 0, 0, 0,
-    0, 0, pow(0.0005 / 100.0, 2.0), 0;
+    0, 0, pow(0.005 / 100.0, 2.0), 0;
   P <<
     pow(100.0, 2.0), 0, 0, 0,
     0, pow(100.0, 2.0), 0, 0,