still small deltas in x

Author: mcm001

src/mrcal-uncertainty.hpp

@@ -310,62 +310,13 @@ Eigen::Matrix<double, 2, Eigen::Dynamic, Eigen::RowMajor> _dq_db_projection_unce
 }
 
 double _observed_pixel_uncertainty_from_inputs(
-    const std::vector<double> &intrinsics,
-    const std::vector<mrcal_pose_t> &rt_ref_frame,
-    const mrcal_observation_board_t *observations_board,
-    const mrcal_point3_t *observations_board_pool,
-    int Nobservations_board,
-    const mrcal_lensmodel_t *lensmodel,
-    const mrcal_problem_selections_t &problem_selections,
-    int calibration_object_width_n,
-    int calibration_object_height_n,
-    double calibration_object_spacing)
+    std::vector<double> &x, int num_measurements_board, int measurement_index_board)
 {
-    int Nstate = mrcal_num_states(
-        1, 0, rt_ref_frame.size(), 0, 0,
-        Nobservations_board, problem_selections, lensmodel);
-
-    int Nmeasurements_boards = mrcal_num_measurements(
-        Nobservations_board, 0, 
-        NULL, 0,
-        calibration_object_width_n, calibration_object_height_n,
-        1, 0, 6, 0, 0, problem_selections, lensmodel);
-
-    std::vector<double> b_packed(Nstate);
-    std::vector<double> x(Nmeasurements_boards);
-
-    double point_min_range = -1.0, point_max_range = -1.0;
-    mrcal_problem_constants_t problem_constants = {
-        .point_min_range = point_min_range, .point_max_range = point_max_range};
-
-    bool ret = mrcal_optimizer_callback(
-        b_packed.data(), Nstate,
-        x.data(), Nmeasurements_boards,
-        nullptr, // no Jacobian needed
-        intrinsics.data(),
-        nullptr, rt_ref_frame.data(),
-        nullptr, nullptr,
-        1, 0, static_cast<int>(rt_ref_frame.size()),
-        0, 0,
-        observations_board, nullptr,
-        Nobservations_board, 0,
-        nullptr, 0,
-        observations_board_pool, nullptr,
-        lensmodel, nullptr,
-        problem_selections, &problem_constants,
-        calibration_object_spacing,
-        calibration_object_width_n, calibration_object_height_n,
-        false);
-
-    if (!ret)
-    {
-        throw std::runtime_error("mrcal_optimizer_callback failed");
-    }
-
     // Compute variance from residuals
     double sum = 0.0, sum_sq = 0.0;
-    for (double val : x)
+    for (size_t i = measurement_index_board; i < measurement_index_board + num_measurements_board; i++)
     {
+        double val = x[i];
         sum += val;
         sum_sq += val * val;
     }
@@ -427,16 +378,18 @@ double _propagate_calibration_uncertainty(
         1, 0, rt_ref_frame.size(), 0, 0,
         Nobservations_board, problem_selections, &lensmodel);
 
-    int Nmeasurements_boards = mrcal_num_measurements(
-        Nobservations_board, 0, 
+    int Nmeasurements = mrcal_num_measurements(
+        Nobservations_board, 0,
         NULL, 0,
         calibration_object_width_n, calibration_object_height_n,
         1, 0, 6, 0, 0, problem_selections, &lensmodel);
+    int Nmeasurements_boards = mrcal_num_measurements_boards(
+        Nobservations_board,
+        calibration_object_width_n, calibration_object_height_n);
+    int imeas0 = mrcal_measurement_index_boards(0, Nobservations_board, 0, calibration_object_width_n, calibration_object_height_n);
 
     // Allocate buffers for Jt sparse matrix
-    int Nmeasurements = Nmeasurements_boards; // Total measurements (boards only for now)
     int N_j_nonzero = Nstate * Nmeasurements; // Upper bound, actual will be less
-
     std::vector<int32_t> Jt_p(Nmeasurements + 1);
     std::vector<int32_t> Jt_i(N_j_nonzero);
     std::vector<double_t> Jt_x(N_j_nonzero);
@@ -468,15 +421,15 @@ double _propagate_calibration_uncertainty(
     cholmod_start(&common);
 
     // TODO fill this in
-    mrcal_calobject_warp_t warp { .x2 = 0.0, .y2 = 0.0 };
+    mrcal_calobject_warp_t warp{.x2 = 0.0, .y2 = 0.0};
 
     // TODO fill this in
-    int imagersize[2] {1280, 720};
+    int imagersize[2]{1280, 720};
 
     bool ret = mrcal_optimizer_callback(
         b_packed.data(), b_packed.size() * sizeof(double), // out
-        x.data(), x.size() * sizeof(double), // out
-        &Jt, // Get the Jacobian
+        x.data(), x.size() * sizeof(double),               // out
+        &Jt,                                               // Get the Jacobian
         // IN parameters
         intrinsics.data(),
         nullptr, // no extrinsics
@@ -501,11 +454,7 @@ double _propagate_calibration_uncertainty(
         throw std::runtime_error("mrcal_optimizer_callback failed");
     }
 
-    double observed_pixel_uncertainty = _observed_pixel_uncertainty_from_inputs(
-        intrinsics, rt_ref_frame, observations_board, observations_board_pool,
-        Nobservations_board, &lensmodel, problem_selections,
-        calibration_object_width_n, calibration_object_height_n,
-        calibration_object_spacing);
+    double observed_pixel_uncertainty = _observed_pixel_uncertainty_from_inputs(x, Nmeasurements_boards, imeas0);
 
     // Create factorization from Jt
     cholmod_factor *factorization = create_factorization(&Jt, &common);
@@ -629,7 +578,7 @@ bool projection_uncertainty(mrcal_point3_t pcam,
     auto dq_db{_dq_db_projection_uncertainty(pcam, lensmodel, rt_ref_frames, Nstate, istate_frames0, intrinsics)};
 
     return _propagate_calibration_uncertainty(dq_db, problem_selections, lensmodel,
-                                              intrinsics, rt_ref_frames, c_observations_board, reinterpret_cast<const mrcal_point3_t*>(observations_board.data()),
+                                              intrinsics, rt_ref_frames, c_observations_board, reinterpret_cast<const mrcal_point3_t *>(observations_board.data()),
                                               rt_ref_frames.size(), calibration_object_width_n_example, calibration_object_height_n_example, calibration_object_spacing_example);
 }