Skip to content

Commit

Permalink
Revert
Browse files Browse the repository at this point in the history
  • Loading branch information
@Gold856
Gold856 committed Oct 22, 2024
1 parent 4971bfc commit 7cfa5b0
Show file tree
Hide file tree
Showing 14 changed files with 1,160 additions and 2,793 deletions.
2 changes: 1 addition & 1 deletion upstream_utils/mrcal.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,7 @@ def copy_upstream_src(wpilib_root):
def main():
name = "mrcal"
url = "https://github.com/dkogan/mrcal"
tag = "71c89c4e9f268a0f4fb950325e7d551986a281ec"
tag = "0d5426b5851be80dd8e51470a0784a73565a3006"

mrcal = Lib(name, url, tag, copy_upstream_src)
mrcal.main()
Expand Down
22 changes: 0 additions & 22 deletions wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,27 +6,6 @@
// until I do that.


// Upper triangle is stored, in the usual row-major order.
__attribute__((unused))
static
int index_sym33(int i, int j)
{
// In the top-right triangle I have i<=j, and
// index = N*i+j - sum(i, i=0..i)
// = N*i+j - (i+1)*i/2
// = (N*2 - i - 1)*i/2 + j
const int N=3;
if(i<=j) return (N*2-i-1)*i/2 + j;
else return (N*2-j-1)*j/2 + i;
}
__attribute__((unused))
static
int index_sym33_assume_upper(int i, int j)
{
const int N=3;
return (N*2-i-1)*i/2 + j;
}

// Upper triangle is stored, in the usual row-major order.
__attribute__((unused))
static
Expand Down Expand Up @@ -1950,7 +1929,6 @@ void mul_genNM_genML_accum(// output
#undef _MUL_CORE

// Some common cases into convenient macros
#define mul_gen23_gen33(P,A,B,scale,ACCUM) mul_genNM_genML ## ACCUM(P,3,1, 2,3,3, A,3,1, B,3,1, scale)
#define mul_gen33_gen33(P,A,B,scale,ACCUM) mul_genNM_genML ## ACCUM(P,3,1, 3,3,3, A,3,1, B,3,1, scale)
#define mul_gen33t_gen33(P,A,B,scale,ACCUM) mul_genNM_genML ## ACCUM(P,3,1, 3,3,3, A,1,3, B,3,1, scale)
#define mul_gen33_gen33t(P,A,B,scale,ACCUM) mul_genNM_genML ## ACCUM(P,3,1, 3,3,3, A,3,1, B,1,3, scale)
Expand Down
109 changes: 58 additions & 51 deletions wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -25,72 +25,79 @@

// The splined stereographic models configuration parameters can be used to
// compute the segment size. I cache this computation
typedef struct {
// The distance between adjacent knots (1 segment) is u_per_segment =
// 1/segments_per_u
double segments_per_u;
typedef struct
{
// The distance between adjacent knots (1 segment) is u_per_segment =
// 1/segments_per_u
double segments_per_u;
} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t;

typedef struct {
bool ready;
union {
#define PRECOMPUTED_STRUCT(s, n) mrcal_##s##__precomputed_t s##__precomputed;
MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT);
typedef struct
{
bool ready;
union
{
#define PRECOMPUTED_STRUCT(s,n) mrcal_ ##s##__precomputed_t s##__precomputed;
MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT);
#undef PRECOMPUTED_STRUCT
};
};
} mrcal_projection_precomputed_t;

void _mrcal_project_internal_opencv( // outputs
mrcal_point2_t* q,
mrcal_point3_t* dq_dp, // may be NULL
double* dq_dintrinsics_nocore, // may be NULL

// inputs
const mrcal_point3_t* p, int N, const double* intrinsics, int Nintrinsics);
bool _mrcal_project_internal( // out
mrcal_point2_t* q,
void _mrcal_project_internal_opencv( // outputs
mrcal_point2_t* q,
mrcal_point3_t* dq_dp, // may be NULL
double* dq_dintrinsics_nocore, // may be NULL

// Stored as a row-first array of shape (N,2,3). Each
// trailing ,3 dimension element is a mrcal_point3_t
mrcal_point3_t* dq_dp,
// core, distortions concatenated. Stored as a row-first
// array of shape (N,2,Nintrinsics). This is a DENSE array.
// High-parameter-count lens models have very sparse
// gradients here, and the internal project() function
// returns those sparsely. For now THIS function densifies
// all of these
double* dq_dintrinsics,
// inputs
const mrcal_point3_t* p,
int N,
const double* intrinsics,
int Nintrinsics);
bool _mrcal_project_internal( // out
mrcal_point2_t* q,

// in
const mrcal_point3_t* p, int N, const mrcal_lensmodel_t* lensmodel,
// core, distortions concatenated
const double* intrinsics,
// Stored as a row-first array of shape (N,2,3). Each
// trailing ,3 dimension element is a mrcal_point3_t
mrcal_point3_t* dq_dp,
// core, distortions concatenated. Stored as a row-first
// array of shape (N,2,Nintrinsics). This is a DENSE array.
// High-parameter-count lens models have very sparse
// gradients here, and the internal project() function
// returns those sparsely. For now THIS function densifies
// all of these
double* dq_dintrinsics,

int Nintrinsics, const mrcal_projection_precomputed_t* precomputed);
void _mrcal_precompute_lensmodel_data(
mrcal_projection_precomputed_t* precomputed,
const mrcal_lensmodel_t* lensmodel);
bool _mrcal_unproject_internal( // out
mrcal_point3_t* out,
// in
const mrcal_point3_t* p,
int N,
const mrcal_lensmodel_t* lensmodel,
// core, distortions concatenated
const double* intrinsics,

// in
const mrcal_point2_t* q, int N, const mrcal_lensmodel_t* lensmodel,
// core, distortions concatenated
const double* intrinsics,
const mrcal_projection_precomputed_t* precomputed);
int Nintrinsics,
const mrcal_projection_precomputed_t* precomputed);
void _mrcal_precompute_lensmodel_data(mrcal_projection_precomputed_t* precomputed,
const mrcal_lensmodel_t* lensmodel);
bool _mrcal_unproject_internal( // out
mrcal_point3_t* out,

// Report the number of non-zero entries in the optimization jacobian
int _mrcal_num_j_nonzero(int Nobservations_board, int Nobservations_point,

// May be NULL if we don't have any of these
const mrcal_observation_point_triangulated_t*
observations_point_triangulated,
int Nobservations_point_triangulated,
// in
const mrcal_point2_t* q,
int N,
const mrcal_lensmodel_t* lensmodel,
// core, distortions concatenated
const double* intrinsics,
const mrcal_projection_precomputed_t* precomputed);

// Report the number of non-zero entries in the optimization jacobian
int _mrcal_num_j_nonzero(int Nobservations_board,
int Nobservations_point,
int calibration_object_width_n,
int calibration_object_height_n,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes, int Npoints, int Npoints_fixed,
int Nframes,
int Npoints, int Npoints_fixed,
const mrcal_observation_board_t* observations_board,
const mrcal_observation_point_t* observations_point,
mrcal_problem_selections_t problem_selections,
Expand Down
55 changes: 13 additions & 42 deletions wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -187,7 +187,6 @@ typedef struct
// is used to identify a specific camera, while the "extrinsics" index is used
// to locate a camera in space. If I have a camera that is moving over time, the
// intrinsics index will remain the same, while the extrinsics index will change
#warning "triangulated-solve: there should be a pool of these, and I should be indexing that pool"
typedef struct
{
// indexes the intrinsics array
Expand Down Expand Up @@ -219,38 +218,21 @@ typedef struct
// indexes the "points" array to select the position of the point being
// observed
int i_point;
} mrcal_observation_point_t;

#warning "triangulated-solve: triangulated points into a pool. maybe allowing the intrinsics to move in the process"

// An observation of a discrete point where the point itself is NOT a part of
// the optimization, but computed implicitly via triangulation. This structure
// is very similar to mrcal_observation_point_t, except instead of i_point
// identifying the point being observed we have
#warning "triangulated-solve: FINISH DOC"
typedef struct
{
// which camera is making this observation
mrcal_camera_index_t icam;

#warning "triangulated-solve: DOCUMENT. CAN THIS BIT FIELD BE PACKED NICELY?"
// Set if this is the last camera observing this point. Any set of N>=2
// cameras can observe any point. All observations of a given point are
// stored consecutively, the last one being noted by this bit
#warning "triangulated-solve: do I really need this? I cannot look at the next observation to determine when this one is done?"
bool last_in_set : 1;

#warning "triangulated-solve: this is temporary. Should be a weight in observations_point_pool like all the other observations"
bool outlier : 1;

// Observed pixel coordinates. This works just like elements of
// observations_board_pool and observations_point_pool
// observations_board_pool:
//
// .x, .y are the pixel observations
// .z is the weight of the observation. Most of the weights are expected to
// be 1.0. Less precise observations have lower weights.
// .z<0 indicates that this is an outlier. This is respected on
// input
//
// Unlike observations_board_pool, outlier rejection is NOT YET IMPLEMENTED
// for points, so outlier points will NOT be found and reported on output in
// .z<0
mrcal_point3_t px;
} mrcal_observation_point_triangulated_t;


#warning "triangulated-solve: need a function to identify a vanilla calibration problem. It needs to not include any triangulated points. The noise propagation is different"

} mrcal_observation_point_t;

// Bits indicating which parts of the optimization problem being solved. We can
// ask mrcal to solve for ALL the lens parameters and ALL the geometry and
Expand All @@ -275,18 +257,13 @@ typedef struct
// If true, optimize the shape of the calibration object
bool do_optimize_calobject_warp : 1;

#warning "triangulated-solve: Need finer-grained regularization flags"
#warning "triangulated-solve: Regularization flags should reflect do_optimize stuff and Ncameras stuff"
// If true, apply the regularization terms in the solver
bool do_apply_regularization : 1;

// Whether to try to find NEW outliers. The outliers given on
// input are respected regardless
bool do_apply_outlier_rejection : 1;

// Pull the distance between the first two cameras to 1.0
bool do_apply_regularization_unity_cam01: 1;

} mrcal_problem_selections_t;

// Constants used in a mrcal optimization. This is similar to
Expand Down Expand Up @@ -319,13 +296,7 @@ typedef struct
/* How many pixel observations were thrown out as outliers. Each pixel */ \
/* observation produces two measurements. Note that this INCLUDES any */ \
/* outliers that were passed-in at the start */ \
_(int, Noutliers_board, PyLong_FromLong) \
\
/* How many pixel observations were thrown out as outliers. Each pixel */ \
/* observation produces two measurements. Note that this INCLUDES any */ \
/* outliers that were passed-in at the start */ \
_(int, Noutliers_triangulated_point, PyLong_FromLong)
#warning "triangulated-solve: implement stats.Noutliers_triangulated_point; add to c-api.org"
_(int, Noutliers, PyLong_FromLong)
#define MRCAL_STATS_ITEM_DEFINE(type, name, pyconverter) type name;
typedef struct
{
Expand Down
Loading

0 comments on commit 7cfa5b0

Please sign in to comment.