Upgrade calibrate jet

filtering/calibrate_jet.cc

@@ -1,26 +1,26 @@
-#include "filtering/extract_data_from_log.hh"
-#include "filtering/to_time_point.hh"
-
 //%deps(jet_filter)
 //%deps(jet_optimizer)
 //%deps(nonlinear_camera_model)
 //%deps(warn_sensor_rates)
+//%deps(make_interpolator)
+#include "third_party/experiments/estimation/sensors/make_interpolator.hh"
+
 #include "third_party/experiments/estimation/calibration/nonlinear_camera_model.hh"
 #include "third_party/experiments/estimation/calibration/warn_sensor_rates.hh"
 #include "third_party/experiments/estimation/jet/jet_filter.hh"
 #include "third_party/experiments/estimation/jet/jet_optimizer.hh"
 
 //%deps(calibrate_single_imu)
-#include "third_party/experiments/estimation/calibration/calibrate_single_imu.hh"
-
-//%deps(make_interpolator)
-#include "third_party/experiments/estimation/sensors/make_interpolator.hh"
-
 //%deps(robust_pnp)
-#include "third_party/experiments/estimation/vision/robust_pnp.hh"
-
 //%deps(visualize_calibration)
+//%deps(visualize_camera_calibration)
+#include "third_party/experiments/estimation/calibration/calibrate_single_imu.hh"
+#include "third_party/experiments/estimation/vision/robust_pnp.hh"
 #include "third_party/experiments/estimation/visualization/visualize_calibration.hh"
+#include "third_party/experiments/estimation/visualization/visualize_camera_calibration.hh"
+
+//%deps(form_coordinate_frame)
+#include "third_party/experiments/geometry/spatial/form_coordinate_frame.hh"
 
 //%deps(simple_geometry)
 //%deps(ui2d)
@@ -29,12 +29,14 @@
 #include "third_party/experiments/viewer/primitives/simple_geometry.hh"
 #include "third_party/experiments/viewer/window_3d.hh"
 
-#include "camera/camera_manager.hh"
-// #include "filtering/visualization/visualize_calibration.hh"
-
+//%deps(jet_model)
+#include "third_party/experiments/planning/jet/jet_model.hh"
 //%deps(log)
 #include "third_party/experiments/logging/log.hh"
 
+#include "filtering/extract_data_from_log.hh"
+
+#include "camera/camera_manager.hh"
 #include "vision/fiducial_detection_and_pose.hh"
 
 // - Validate reprojection of fiducial detection using the packaged calibration
@@ -47,6 +49,8 @@ namespace jet {
 namespace filtering {
 
 namespace {
+constexpr int IMU_1 = 78;
+constexpr int IMU_2 = 36;
 
 estimation::ProjectionCoefficients proj_coeffs_from_opencv(const Calibration& cal) {
   estimation::ProjectionCoefficients model;
@@ -87,17 +91,30 @@ void align_crappy_jake(const std::shared_ptr<viewer::SimpleGeometry>& geo,
   std::vector<jcc::Vec2> observed;
   std::vector<jcc::Vec3> object;
 
-  SE3 camera_from_fiducial = fiducial_from_camera_init.inverse();
   const auto associations = object_image_assoc_from_board_point(obj_pt_associations);
 
   const auto pnp_visitor = estimation::make_pnp_visitor(model, object, observed);
   const auto pnp_result = estimation::robust_pnp(model, fiducial_from_camera_init, object, observed, pnp_visitor);
 }
+
 }  // namespace
-const bool visualize_imu_model = true;
-const bool visualize_camera = true;
+
+const estimation::CreateSingleImuModelConfig imu_cal_cfg{
+    .visualize_imu_model = false,    //
+    .visualize_gyro = false,         //
+    .visualize_magnetometer = false  //
+};
+
+const estimation::CameraCalibrationConfig camera_cal_cfg{
+    .visualize_camera = false,             //
+    .visualize_camera_distortion = false,  //
+    .visualize_camera_frustum = false      //
+};
+
 bool visualize() {
-  return (visualize_imu_model || visualize_camera);
+  return (camera_cal_cfg.visualize_camera || camera_cal_cfg.visualize_camera_distortion ||
+          camera_cal_cfg.visualize_camera_frustum || imu_cal_cfg.visualize_imu_model || imu_cal_cfg.visualize_gyro ||
+          imu_cal_cfg.visualize_magnetometer);
 }
 
 void setup() {
@@ -112,19 +129,164 @@ void setup() {
 }
 
 struct JetModel {
-  std::map<int, estimation::SingleImuCalibration> imu_model_from_imu_id;
+  std::map<int, estimation::SingleImuCalibration> imu_calibration_from_imu_id;
 };
 
+jcc::Optional<ejf::FiducialMeasurement> find_nearest_fiducial(
+    const estimation::calibration::CalibrationMeasurements& cal_meas, const estimation::TimePoint& t) {
+  JASSERT_FALSE(cal_meas.fiducial_meas.empty(), "Cannot calibrate with empty fiducial measurements");
+  for (const auto& fiducial_meas : cal_meas.fiducial_meas) {
+    if (t < fiducial_meas.timestamp) {
+      return {fiducial_meas.measurement};
+    }
+  }
+  return {};
+}
+
+SE3 compute_world_from_camera(const estimation::SingleImuCalibration& imu_cal) {
+  const geometry::Unit3 g_camera_frame = imu_cal.camera_from_gyro * imu_cal.g_estimate.direction;
+  const SO3 world_from_camera = geometry::spatial::form_coordinate_frame_from_zhat(g_camera_frame);
+  return SE3(world_from_camera, jcc::Vec3::Zero());
+}
+
+ejf::Parameters compute_parameters(const estimation::calibration::CalibrationMeasurements& cal_meas,
+                                   const JetModel& jet_model) {
+  const auto& imu_cal_1 = jet_model.imu_calibration_from_imu_id.at(IMU_1);
+  const auto& imu_cal_2 = jet_model.imu_calibration_from_imu_id.at(IMU_2);
+
+  const SO3 world_from_camera_1 = compute_world_from_camera(imu_cal_1).so3();
+  const SO3 world_from_camera_2 = compute_world_from_camera(imu_cal_2).so3();
+  jcc::Warning() << "[Filter Setup] IMU->IMU Alignment Error: "
+                 << (world_from_camera_1 * world_from_camera_2.inverse()).log().norm() << std::endl;
+
+  const auto maybe_nearest_fiducial = find_nearest_fiducial(cal_meas, imu_cal_1.g_estimate.time);
+  assert(maybe_nearest_fiducial);
+  const auto fiducial_at_g_estimate = *maybe_nearest_fiducial;
+  const SO3 fiducial_from_camera = fiducial_at_g_estimate.T_fiducial_from_camera.so3();
+  const SO3 world_from_fiducial = world_from_camera_1 * fiducial_from_camera.inverse();
+
+  auto p = ejf::JetFilter::reasonable_parameters();
+  {
+    p.T_world_from_fiducial = SE3(world_from_fiducial, jcc::Vec3::Zero());
+    p.T_imu1_from_vehicle = SE3(imu_cal_1.camera_from_gyro.inverse(), jcc::Vec3::Zero());
+    p.T_imu2_from_vehicle = SE3(imu_cal_2.camera_from_gyro.inverse(), jcc::Vec3::Zero());
+  }
+
+  return p;
+}
+
+void test_filter(const estimation::calibration::CalibrationMeasurements& cal_meas, const JetModel& jet_model) {
+  const auto view = viewer::get_window3d("Calibration");
+  const auto geo = view->add_primitive<viewer::SimpleGeometry>();
+
+  const auto geo_stable = view->add_primitive<viewer::SimpleGeometry>();
+  const auto jet_tree = view->add_primitive<viewer::SceneTree>();
+
+  const jcc::Vec3 jet_origin(1.0, 0.0, 0.0);
+
+  const planning::jet::JetModel jet_3dmodel;
+  constexpr bool DRAW_VEHICLE = true;
+  const SO3 jetmodel_from_body = jcc::exp_x(M_PI * 0.5).so3();
+  if constexpr (DRAW_VEHICLE) {
+    jet_3dmodel.insert(*jet_tree);
+  }
+
+  const auto& imu_1_meas = cal_meas.imu_cal.at(IMU_1);
+  const auto& imu_1_cal = jet_model.imu_calibration_from_imu_id.at(IMU_1);
+  const auto& imu_2_cal = jet_model.imu_calibration_from_imu_id.at(IMU_2);
+
+  const auto& imu_1_model = imu_1_cal.imu_model;
+  const auto accel_interp = estimation::make_accel_interpolator(imu_1_meas.accel_meas, imu_1_model);
+
+  const auto t0 = cal_meas.first();
+  // const auto t0 = imu_1_meas.accel_meas.front().timestamp;
+
+  auto xp0 = ejf::JetFilter::reasonable_initial_state(t0);
+  {  //
+    xp0.x.R_world_from_body = compute_world_from_camera(jet_model.imu_calibration_from_imu_id.at(IMU_2)).so3();
+  }
+  const auto p = compute_parameters(cal_meas, jet_model);
+  ejf::JetFilter jf(xp0, p);
+
+  for (const auto& fiducial_meas : cal_meas.fiducial_meas) {
+    jf.measure_fiducial(fiducial_meas.measurement, fiducial_meas.timestamp);
+  }
+
+  for (const auto& accel : cal_meas.imu_cal.at(IMU_1).accel_meas) {
+    const jcc::Vec3 corrected_accel = imu_1_model.correct_measured_accel(accel.measurement.observed_acceleration);
+    // jf.measure_imu({corrected_accel}, accel.timestamp);
+  }
+
+  for (const auto& gyro : cal_meas.imu_cal.at(IMU_1).gyro_meas) {
+    jf.measure_gyro(gyro.measurement, gyro.timestamp);
+  }
+
+  const auto& imu_2_model = imu_2_cal.imu_model;
+  for (const auto& accel : cal_meas.imu_cal.at(IMU_2).accel_meas) {
+    const jcc::Vec3 corrected_accel = imu_2_model.correct_measured_accel(accel.measurement.observed_acceleration);
+    // jf.measure_imu({corrected_accel}, accel.timestamp, true);
+  }
+
+  for (const auto& gyro : cal_meas.imu_cal.at(IMU_2).gyro_meas) {
+    // jf.measure_gyro(gyro.measurement, gyro.timestamp, true);
+  }
+
+  int k = 0;
+  while (jf.next_measurement()) {
+    ++k;
+    if (k % 2 != 0) {
+      continue;
+    }
+    const auto state = jf.state().x;
+    // geo->add_axes({get_world_from_body(state)});
+
+    const SE3 T_world_from_body = get_world_from_body(state);
+    constexpr bool PRINT_STATES = false;
+    if (PRINT_STATES) {
+      std::cout << "States:       " << std::endl;
+      std::cout << "\taccel_bias: " << state.accel_bias.transpose() << std::endl;
+      std::cout << "\tgyro_bias: " << state.gyro_bias.transpose() << std::endl;
+      std::cout << "\teps_ddot:   " << state.eps_ddot.transpose() << std::endl;
+      std::cout << "\teps_dot:    " << state.eps_dot.transpose() << std::endl;
+      std::cout << "\tr:          " << T_world_from_body.so3().log().transpose() << std::endl;
+      std::cout << "\tx:          " << T_world_from_body.translation().transpose() << std::endl;
+    }
+
+    // Fix jitter: Covariances plz bby
+    geo->add_axes({SE3(T_world_from_body.so3(), jet_origin)});
+
+    const jcc::Vec3 accel_imu_frame = *accel_interp(jf.state().time_of_validity);
+
+    const SE3 imu_from_vehicle = jf.parameters().T_imu1_from_vehicle;
+    const SO3 world_from_imu = state.R_world_from_body * imu_from_vehicle.so3().inverse();
+
+    geo->add_line(
+        {jet_origin, jet_origin + (world_from_imu * accel_imu_frame) / 9.81, jcc::Vec4(0.2, 0.2, 1.0, 1.0), 5.0});
+
+    geo->add_line({jcc::Vec3::Zero(), accel_imu_frame, jcc::Vec4(0.2, 0.2, 1.0, 1.0), 5.0});
+    // Velocity
+    geo->add_line({jcc::Vec3::Zero(), state.eps_dot.head<3>(), jcc::Vec4(1.0, 0.0, 0.0, 1.0), 5.0});
+    // Angular Velocity
+    geo->add_line({jcc::Vec3::Zero(), -state.eps_dot.tail<3>(), jcc::Vec4(0.0, 1.0, 0.0, 1.0), 5.0});
+
+    jet_tree->set_world_from_root(SE3(T_world_from_body.so3() * jetmodel_from_body.inverse(), jet_origin));
+
+    geo->flip();
+    view->spin_until_step();
+  }
+}
+
 void go() {
   jcc::Success() << "Preparing to calibrate" << std::endl;
 
   //
   // Grab the calibration data from the log
   //
 
-  // const std::string path = "/jet/logs/jet-calibration-log-jun30-1/";
-  const std::string path = "/jet/logs/calibration-log-jul4-2/";
-  // const std::string path = "/jet/logs/calibration-log-jul5-4";
+  // const std::string path = "/jet/logs/calibration-log-jul4-2/";
+
+  // const std::string path = "/jet/logs/calibration-log-jul6-6";
+  const std::string path = "/jet/logs/calibration-log-jul6-7";
 
   const TimeRange range{};
   const ExtractionConfiguration default_cfg{};
@@ -149,34 +311,38 @@ void go() {
   jcc::Success() << "Calibrating..." << std::endl;
 
   //
-  // Calibrate the IMU intrinsics
+  // Calibrate the IMU intrinsics / Estrinsics
   //
 
-  const auto imu_measurements = cal_measurements.imu_cal.begin()->second;
-  const auto one_imu = estimation::create_single_imu_model(cal_measurements, imu_measurements);
+  JetModel jet_model;
+  for (const auto& imu_measurements : cal_measurements.imu_cal) {
+    const auto one_imu = estimation::create_single_imu_model(cal_measurements, imu_measurements.second, imu_cal_cfg);
+    jet_model.imu_calibration_from_imu_id[imu_measurements.first] = one_imu;
+  }
 
   //
   // Validate the fiducial reprojections
   //
 
   jcc::Success() << "[Camera] Validating fiducial measurements" << std::endl;
   ImageStream image_stream(path, range);
-  jcc::Success() << "[Camera] Parsing images" << std::endl;
+  jcc::Success() << "[Camera] Parsing images..." << std::endl;
 
   const CameraManager cam_mgr;
   const auto view = viewer::get_window3d("Calibration");
   const auto geo = view->add_primitive<viewer::SimpleGeometry>();
-
-  const auto geo2 = view->add_primitive<viewer::SimpleGeometry>();
-
   const auto ui2d = view->add_primitive<viewer::Ui2d>();
-  const auto accel_interp = estimation::make_accel_interpolator(imu_measurements.accel_meas, one_imu.imu_model);
 
   bool set = false;
   geometry::Unit3 permanent_g_fiducial_frame;
 
+  std::vector<estimation::calibration::TimedMeasurement<ejf::FiducialMeasurement>> collected_fiducial_meas;
+
+  int i = 0;
   while (true) {
     const auto image = image_stream.next();
+
+    ++i;
     if (!image) {
       break;
     }
@@ -188,60 +354,27 @@ void go() {
     const auto obj_pt_associations = obj_points_img_points_from_image(image->image);
     const auto fiducial_from_camera = estimate_board_bottom_left_from_camera(image->image, calibration);
 
-    if (visualize_camera) {
-      ui2d->clear();
-      geo->clear();
-      if (fiducial_from_camera) {
-        const auto associations = object_image_assoc_from_board_point(obj_pt_associations);
-        estimation::visualize_fiducial_detection(geo, ui2d, model, *fiducial_from_camera, associations);
-      } else {
-        ui2d->add_image(image->image, 1.0);
-        ui2d->flip();
-        view->spin_until_step();
-        continue;
-      }
-
-      const auto t = image->time;
-      const auto meas_accel = accel_interp(t);
-
-      if (meas_accel) {
-        const geometry::Unit3 g_gyro_frame = geometry::Unit3(*meas_accel);
-        const geometry::Unit3 g_camera_frame = one_imu.camera_from_gyro * g_gyro_frame;
-        const geometry::Unit3 g_fiducial_frame = fiducial_from_camera->so3() * g_camera_frame;
-
-        if (!set) {
-          set = true;
-          permanent_g_fiducial_frame = g_fiducial_frame;
-          std::cout << "Updating G fiducial frame" << std::endl;
-        }
-
-        // if (obj_pt_associations.size() < 32) {
-          // std::cout << "BAD" << std::endl;
-          // continue;
-        // }
-
-        const geometry::Unit3 g0_fiducial_frame =
-            fiducial_from_camera->so3() * one_imu.camera_from_gyro * one_imu.g_estimate.direction;
-        const jcc::Vec3 fiducial_origin = fiducial_from_camera->inverse().translation();
-        const geometry::Unit3 fiducial_x_hat_camera_frame =
-            fiducial_from_camera->so3().inverse() * geometry::Unit3::UnitX();
-
-        geo->add_axes({SE3(one_imu.camera_from_gyro, jcc::Vec3(0.0, 0.0, 1.0))});
-      }
-      ui2d->add_image(image->image, 1.0);
-      ui2d->flush();
-      // align_crappy_jake(geo, ui2d, model, *fiducial_from_camera, obj_pt_associations);
-
-      estimation::visualize_camera_distortion(ui2d, *image, model);
-      estimation::visualize_camera_frustum(geo, ui2d, *image, model);
-
-      ui2d->flip();
-      geo->flip();
-
-      view->spin_until_step();
+    const auto t = image->time;
+    if (fiducial_from_camera) {
+      ejf::FiducialMeasurement fiducial_measurement;
+      fiducial_measurement.T_fiducial_from_camera = *fiducial_from_camera;
+      collected_fiducial_meas.push_back({fiducial_measurement, t});
+    }
+
+    // TODO: NEXT, FILL THIS IN
+    if (camera_cal_cfg.visualize_camera) {
+      const auto associations = object_image_assoc_from_board_point(obj_pt_associations);
+      estimation::visualize_single_camera_frame(
+          model, fiducial_from_camera, associations, *image, ui2d, geo, camera_cal_cfg);
     }
   }
 
+  jcc::Success() << "[Filter] Testing filter..." << std::endl;
+  {
+    estimation::calibration::CalibrationMeasurements new_cal_measurements = cal_measurements;
+    new_cal_measurements.fiducial_meas = collected_fiducial_meas;
+    test_filter(new_cal_measurements, jet_model);
+  }
   jcc::Success() << "Done." << std::endl;
 }