Skip to content

Commit 0982adf

Browse files
mcm001mcm001
committed
waah mrcal_optimizer_callback has so many args
1 parent 8bfbbce commit 0982adf

File tree

1 file changed

+136
-29
lines changed

1 file changed

+136
-29
lines changed

‎src/mrcal-uncertainty.hpp‎

Lines changed: 136 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -85,9 +85,9 @@ std::vector<mrcal_point2_t> sample_imager(Size numSamples, Size imagerSize)
8585
Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
8686
mrcal_point3_t pcam,
8787
mrcal_lensmodel_t lensmodel,
88-
std::vector<mrcal_pose_t>& rt_ref_frame,
88+
std::vector<mrcal_pose_t> &rt_ref_frame,
8989
int Nstate, int istate_frames0,
90-
std::vector<double>& intrinsics)
90+
std::vector<double> &intrinsics)
9191
{
9292
// project with gradients
9393
// model_analysis.py:1067
@@ -97,7 +97,7 @@ Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
9797
bool ret = mrcal_project(
9898
// out
9999
&q,
100-
reinterpret_cast<mrcal_point3_t*>(dq_dpcam.data()),
100+
reinterpret_cast<mrcal_point3_t *>(dq_dpcam.data()),
101101
dq_dintrinsics.data(),
102102
// in
103103
&pcam, 1,
@@ -109,10 +109,10 @@ Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
109109
}
110110

111111
// number of boards
112-
const size_t Nboards {rt_ref_frame.size()};
112+
const size_t Nboards{rt_ref_frame.size()};
113113

114114
// p_ref = pcam rotated by r (always zero1)
115-
Eigen::Matrix<double, 1, 3> p_ref {pcam.x, pcam.y, pcam.z};
115+
Eigen::Matrix<double, 1, 3> p_ref{pcam.x, pcam.y, pcam.z};
116116

117117
// fmt::print("_dq_db_projection_uncertainty: ==========\n");
118118
// fmt::print("q={}\n", q);
@@ -130,14 +130,13 @@ Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
130130
dq_db.leftCols(intrinsics.size()) = dq_dintrinsics_eigen;
131131

132132
// determine dpcam_dr and dpcamp_dpref
133-
Eigen::Matrix<double, 3, 3> dpcam_dpref; //dxout/dxin
134-
Eigen::Matrix<double, 3, 3> dpcam_dr; // dx_out/dr
133+
Eigen::Matrix<double, 3, 3> dpcam_dpref; // dxout/dxin
134+
Eigen::Matrix<double, 3, 3> dpcam_dr; // dx_out/dr
135135
{
136136
// HACK -- hard-coded to origin
137137
Eigen::Matrix<double, 1, 6> rt_cam_ref;
138138
rt_cam_ref.setZero();
139139

140-
141140
// output arrays
142141
mrcal_point3_t rotated_p_ref;
143142

@@ -148,12 +147,11 @@ Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
148147
dpcam_dpref.data(),
149148
// in
150149
rt_cam_ref.data(),
151-
p_ref.data()
152-
);
150+
p_ref.data());
153151
}
154152

155153
// method is always mean-pcam
156-
Eigen::Matrix<double, 2, 3> dq_dpref = dq_dpcam * dpcam_dpref;
154+
Eigen::Matrix<double, 2, 3> dq_dpref = dq_dpcam * dpcam_dpref;
157155

158156
// calculate p_frames
159157
Eigen::Matrix<double, Eigen::Dynamic, 3, Eigen::RowMajor> p_frames(Nboards, 3);
@@ -165,14 +163,12 @@ Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
165163
{
166164
mrcal_rotate_point_r_inverted(
167165
// out
168-
p_frames.row(pose).data(),
166+
p_frames.row(pose).data(),
169167
NULL, NULL,
170168
// in
171169
rt_ref_frame[pose].r.xyz,
172-
p_ref.data()
173-
);
170+
p_ref.data());
174171
}
175-
176172
}
177173

178174
// and rotate to yield dpref_dframes
@@ -188,8 +184,7 @@ Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
188184
NULL,
189185
// in
190186
rt_ref_frame[pose].r.xyz,
191-
p_frames.row(pose).data()
192-
);
187+
p_frames.row(pose).data());
193188
}
194189

195190
// Calculate dq_dframes
@@ -199,7 +194,7 @@ Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
199194
{
200195
dq_dframes[pose] = dq_dpref * dpref_dframes[pose];
201196
}
202-
197+
203198
// Populate dq_db_slice_frames
204199
// Shape after mean and xchg: (2, 3) for at_infinity
205200
// Each frame gets 3 DOF (translation only)
@@ -230,17 +225,130 @@ Eigen::Matrix<double, 2, Eigen::Dynamic> _dq_db_projection_uncertainty(
230225
// std::cout << "}\n";
231226

232227
Eigen::IOFormat CommaFmt(Eigen::StreamPrecision, Eigen::DontAlignCols, ", ", "\n", "", "");
233-
std::cout << "dq_db final:\n" << dq_db.format(CommaFmt) << "\n";
228+
std::cout << "dq_db final:\n"
229+
<< dq_db.format(CommaFmt) << "\n";
234230

235231
return dq_db;
236232
}
237233

238-
bool _propagate_calibration_uncertainty(Eigen::Matrix<double, 2, Eigen::Dynamic> dF_dbunpacked)
234+
void _observed_pixel_uncertainty_from_inputs()
235+
{
236+
// model_analysis.py:511
237+
}
238+
239+
bool _propagate_calibration_uncertainty(Eigen::Matrix<double, 2, Eigen::Dynamic> dF_dbunpacked,
240+
mrcal_problem_selections_t &problem_selections,
241+
mrcal_lensmodel_t &lensmodel)
239242
{
240-
// what = worstdirection-stdev
243+
/* what = worstdirection-stdev */
241244

242245
// pack dF_dbunpacked into dF_dbpacked via mrcal_unpack_state (????)
243246

247+
// void mrcal_unpack_solver_state_vector( // out, in
248+
// double* b, // unitless state on input,
249+
// // scaled, meaningful state on
250+
// // output
251+
252+
// // in
253+
// int Ncameras_intrinsics, int Ncameras_extrinsics,
254+
// int Nframes,
255+
// int Npoints, int Npoints_fixed, int Nobservations_board,
256+
// mrcal_problem_selections_t problem_selections,
257+
// const mrcal_lensmodel_t* lensmodel);
258+
mrcal_unpack_solver_state_vector(
259+
// out, in. modified in place
260+
dF_dbunpacked.data(),
261+
262+
// in
263+
1, // Ncameras_intrinsics
264+
0, // Ncameras_extrinsics
265+
6, // Nframes
266+
0, // Npoints
267+
0, // Npoints_fixed
268+
6, // Nobservations_board
269+
problem_selections,
270+
&lensmodel);
271+
272+
// Get x, jpacked, factorization
273+
274+
// bool mrcal_optimizer_callback(// out
275+
276+
// // These output pointers may NOT be NULL, unlike
277+
// // their analogues in mrcal_optimize()
278+
279+
// // Shape (Nstate,)
280+
// double* b_packed,
281+
// // used only to confirm that the user passed-in the buffer they
282+
// // should have passed-in. The size must match exactly
283+
// int buffer_size_b_packed,
284+
285+
// // Shape (Nmeasurements,)
286+
// double* x,
287+
// // used only to confirm that the user passed-in the buffer they
288+
// // should have passed-in. The size must match exactly
289+
// int buffer_size_x,
290+
291+
// // output Jacobian. May be NULL if we don't need
292+
// // it. This is the unitless Jacobian, used by the
293+
// // internal optimization routines
294+
// struct cholmod_sparse_struct* Jt,
295+
296+
297+
// // in
298+
299+
// // The number of intrinsics parameters varies,
300+
// // depending on lensmodel
301+
// const double* intrinsics, // Ncameras_intrinsics * NlensParams
302+
// const mrcal_pose_t* rt_cam_ref, // Ncameras_extrinsics of these. Transform FROM the reference frame
303+
// const mrcal_pose_t* rt_ref_frame, // Nframes of these. Transform TO the reference frame
304+
// const mrcal_point3_t* points, // Npoints of these. In the reference frame
305+
// const mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp
306+
307+
// int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
308+
// int Npoints, int Npoints_fixed, // at the end of points[]
309+
310+
// const mrcal_observation_board_t* observations_board,
311+
// const mrcal_observation_point_t* observations_point,
312+
// int Nobservations_board,
313+
// int Nobservations_point,
314+
315+
// const mrcal_observation_point_triangulated_t* observations_point_triangulated,
316+
// int Nobservations_point_triangulated,
317+
318+
// // All the board pixel observations, in an array of shape
319+
// //
320+
// // ( Nobservations_board,
321+
// // calibration_object_height_n,
322+
// // calibration_object_width_n )
323+
// //
324+
// // .x, .y are the pixel observations .z is the
325+
// // weight of the observation. Most of the weights
326+
// // are expected to be 1.0. Less precise
327+
// // observations have lower weights.
328+
// //
329+
// // .z<0 indicates that this is an outlier
330+
// const mrcal_point3_t* observations_board_pool,
331+
332+
// // Same this, but for discrete points. Array of shape
333+
// //
334+
// // ( Nobservations_point,)
335+
// const mrcal_point3_t* observations_point_pool,
336+
337+
// const mrcal_lensmodel_t* lensmodel,
338+
// const int* imagersizes, // Ncameras_intrinsics*2 of these
339+
// mrcal_problem_selections_t problem_selections,
340+
// const mrcal_problem_constants_t* problem_constants,
341+
// double calibration_object_spacing,
342+
// int calibration_object_width_n,
343+
// int calibration_object_height_n,
344+
// bool verbose);
345+
346+
{
347+
bool success = mrcal_optimizer_callback(
348+
349+
);
350+
}
351+
244352
// estimate observed_pixel_uncertainty via _observed_pixel_uncertainty_from_inputs()
245353

246354
// call process_slice
@@ -290,17 +398,16 @@ bool projection_uncertainty(mrcal_point3_t pcam,
290398
&lensmodel);
291399

292400
// hard code reference frame transformations
293-
std::vector<mrcal_pose_t> rt_ref_frames {
401+
std::vector<mrcal_pose_t> rt_ref_frames{
294402
{-0.13982929, -0.37331785, -0.01785786, -0.15373499, -0.13686309, 0.59757725},
295-
{-0.18951098, -0.50825451, 0.02212706, -0.26111978, -0.10816078, 0.58305005},
296-
{-0.09580704, -0.4029582, -0.01730795, -0.17221784, -0.17518785, 0.58390618},
297-
{-0.08038678, -0.21667501, -0.00719197, -0.05143006, -0.17069075, 0.59817879},
298-
{-0.11562071, -0.19945448, -0.02150643, -0.02264116, -0.15749109, 0.59493056},
299-
{-0.14950876, -0.05920069, 0.0375357, 0.08856689, -0.10811448, 0.59142776}
300-
};
403+
{-0.18951098, -0.50825451, 0.02212706, -0.26111978, -0.10816078, 0.58305005},
404+
{-0.09580704, -0.4029582, -0.01730795, -0.17221784, -0.17518785, 0.58390618},
405+
{-0.08038678, -0.21667501, -0.00719197, -0.05143006, -0.17069075, 0.59817879},
406+
{-0.11562071, -0.19945448, -0.02150643, -0.02264116, -0.15749109, 0.59493056},
407+
{-0.14950876, -0.05920069, 0.0375357, 0.08856689, -0.10811448, 0.59142776}};
301408

302409
auto dq_db{_dq_db_projection_uncertainty(pcam, lensmodel, rt_ref_frames, Nstate, istate_frames0, intrinsics)};
303-
return _propagate_calibration_uncertainty();
410+
return _propagate_calibration_uncertainty(dq_db, problem_selections, lensmodel);
304411
}
305412

306413
void compute_uncertainty()

0 commit comments

Comments
 (0)