Skip to content
Permalink
bface33421
Switch branches/tags

Name already in use

A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?

mrcal/mrcal.c

Go to file
 
 
Cannot retrieve contributors at this time
5102 lines (4482 sloc) 218 KB
Raw Blame
#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <inttypes.h>
#include <dogleg.h>
#include <minimath.h>
#include <assert.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
#include "mrcal.h"
// These are parameter variable scales. They have the units of the parameters
// themselves, so the optimizer sees x/SCALE_X for each parameter. I.e. as far
// as the optimizer is concerned, the scale of each variable is 1. This doesn't
// need to be precise; just need to get all the variables to be within the same
// order of magnitute. This is important because the dogleg solve treats the
// trust region as a ball in state space, and this ball is isotropic, and has a
// radius that applies in every direction
//
// Can be visualized like this:
//
// p0,x0,J0 = mrcal.optimizer_callback(**optimization_inputs)[:3]
// J0 = J0.toarray()
// ss = np.sum(np.abs(J0), axis=-2)
// gp.plot(ss, _set=mrcal.plotoptions_state_boundaries(**optimization_inputs))
//
// This visualizes the overall effect of each variable. If the scales aren't
// tuned properly, some variables will have orders of magnitude stronger
// response than others, and the optimization problem won't converge well.
//
// The scipy.optimize.least_squares() function claims to be able to estimate
// these automatically, without requiring these hard-coded values from the user.
// See the description of the "x_scale" argument:
//
// https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.least_squares.html
//
// Supposedly this paper describes the method:
//
// J. J. More, "The Levenberg-Marquardt Algorithm: Implementation and Theory,"
// Numerical Analysis, ed. G. A. Watson, Lecture Notes in Mathematics 630,
// Springer Verlag, pp. 105-116, 1977.
//
// Please somebody look at this
#define SCALE_INTRINSICS_FOCAL_LENGTH 500.0
#define SCALE_INTRINSICS_CENTER_PIXEL 20.0
#define SCALE_ROTATION_CAMERA (0.1 * M_PI/180.0)
#define SCALE_TRANSLATION_CAMERA 1.0
#define SCALE_ROTATION_FRAME (15.0 * M_PI/180.0)
#define SCALE_TRANSLATION_FRAME 1.0
#define SCALE_POSITION_POINT SCALE_TRANSLATION_FRAME
#define SCALE_CALOBJECT_WARP 0.01
#define SCALE_DISTORTION 1.0
#define MSG(fmt, ...) fprintf(stderr, "%s(%d): " fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__)
#define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0)
#define CHECK_CONFIG_NPARAM_NOCONFIG(s,n) \
static_assert(n > 0, "no-config implies known-at-compile-time param count");
#define CHECK_CONFIG_NPARAM_WITHCONFIG(s,n) \
static_assert(n <= 0, "with-config implies unknown-at-compile-time param count");
MRCAL_LENSMODEL_NOCONFIG_LIST( CHECK_CONFIG_NPARAM_NOCONFIG)
MRCAL_LENSMODEL_WITHCONFIG_LIST(CHECK_CONFIG_NPARAM_WITHCONFIG)
// Returns a static string, using "..." as a placeholder for any configuration
// values
#define LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \
"_" #name "=..."
#define LENSMODEL_PRINT_CFG_ELEMENT_FMT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \
"_" #name "=%" PRIcode
#define LENSMODEL_PRINT_CFG_ELEMENT_VAR(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \
,config->name
#define LENSMODEL_SCAN_CFG_ELEMENT_FMT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \
"_" #name "=%" SCNcode
#define LENSMODEL_SCAN_CFG_ELEMENT_VAR(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \
,&config->name
#define LENSMODEL_SCAN_CFG_ELEMENT_PLUS1(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \
+1
const char* mrcal_lensmodel_name_unconfigured( mrcal_lensmodel_t model )
{
switch(model.type)
{
#define CASE_STRING_NOCONFIG(s,n) case MRCAL_##s: ; \
return #s;
#define _CASE_STRING_WITHCONFIG(s,n,s_CONFIG_LIST) case MRCAL_##s: ; \
return #s s_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE, );
#define CASE_STRING_WITHCONFIG(s,n) _CASE_STRING_WITHCONFIG(s,n,MRCAL_ ## s ## _CONFIG_LIST)
MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_STRING_NOCONFIG )
MRCAL_LENSMODEL_WITHCONFIG_LIST( CASE_STRING_WITHCONFIG )
default:
assert(0);
#undef CASE_STRING_NOCONFIG
#undef CASE_STRING_WITHCONFIG
}
return NULL;
}
// Write the model name WITH the full config into the given buffer. Identical to
// mrcal_lensmodel_name_unconfigured() for configuration-free models
static int LENSMODEL_SPLINED_STEREOGRAPHIC__snprintf_model
(char* out, int size,
const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config)
{
return
snprintf( out, size, "LENSMODEL_SPLINED_STEREOGRAPHIC"
MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_FMT, )
MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_VAR, ));
}
bool mrcal_lensmodel_name( char* out, int size, mrcal_lensmodel_t model )
{
switch(model.type)
{
#define CASE_STRING_NOCONFIG(s,n) case MRCAL_##s: \
return size > snprintf(out,size, #s);
#define CASE_STRING_WITHCONFIG(s,n) case MRCAL_##s: \
return size > s##__snprintf_model(out, size, &model.s##__config);
MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_STRING_NOCONFIG )
MRCAL_LENSMODEL_WITHCONFIG_LIST( CASE_STRING_WITHCONFIG )
default:
assert(0);
#undef CASE_STRING_NOCONFIG
#undef CASE_STRING_WITHCONFIG
}
return NULL;
}
static bool LENSMODEL_SPLINED_STEREOGRAPHIC__scan_model_config( mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config, const char* config_str)
{
int pos;
int Nelements = 0 MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_PLUS1, );
return
Nelements ==
sscanf( config_str,
MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_FMT, )"%n"
MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_VAR, ),
&pos) &&
config_str[pos] == '\0';
}
const char* const* mrcal_supported_lensmodel_names( void )
{
#define NAMESTRING_NOCONFIG(s,n) #s,
#define _NAMESTRING_WITHCONFIG(s,n,s_CONFIG_LIST) #s s_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE, ),
#define NAMESTRING_WITHCONFIG(s,n) _NAMESTRING_WITHCONFIG(s,n,MRCAL_ ## s ## _CONFIG_LIST)
static const char* names[] = {
MRCAL_LENSMODEL_NOCONFIG_LIST( NAMESTRING_NOCONFIG)
MRCAL_LENSMODEL_WITHCONFIG_LIST(NAMESTRING_WITHCONFIG)
NULL };
return names;
}
#undef LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE
#undef LENSMODEL_PRINT_CFG_ELEMENT_FMT
#undef LENSMODEL_PRINT_CFG_ELEMENT_VAR
#undef LENSMODEL_SCAN_CFG_ELEMENT_FMT
#undef LENSMODEL_SCAN_CFG_ELEMENT_VAR
#undef LENSMODEL_SCAN_CFG_ELEMENT_PLUS1
// parses the model name AND the configuration into a mrcal_lensmodel_t structure.
// Strings with valid model names but missing or unparseable configuration
// return {.type = MRCAL_LENSMODEL_INVALID_BADCONFIG}. Unknown model names return
// {.type = MRCAL_LENSMODEL_INVALID}
mrcal_lensmodel_t mrcal_lensmodel_from_name( const char* name )
{
#define CHECK_AND_RETURN_NOCONFIG(s,n) \
if( 0 == strcmp( name, #s) ) \
return (mrcal_lensmodel_t){.type = MRCAL_##s};
#define CHECK_AND_RETURN_WITHCONFIG(s,n) \
/* Configured model. I need to extract the config from the string. */ \
/* The string format is NAME_cfg1=var1_cfg2=var2... */ \
if( 0 == strcmp( name, #s) ) \
return (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_BADCONFIG}; \
if( 0 == strncmp( name, #s"_", strlen(#s)+1) ) \
{ \
/* found name. Now extract the config */ \
mrcal_lensmodel_t model = {.type = MRCAL_##s}; \
mrcal_##s##__config_t* config = &model.s##__config; \
\
const char* config_str = &name[strlen(#s)]; \
\
if(s##__scan_model_config(config, config_str)) \
return model; \
else \
return (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_BADCONFIG}; \
}
MRCAL_LENSMODEL_NOCONFIG_LIST( CHECK_AND_RETURN_NOCONFIG );
MRCAL_LENSMODEL_WITHCONFIG_LIST( CHECK_AND_RETURN_WITHCONFIG );
return (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID};
#undef CHECK_AND_RETURN_NOCONFIG
#undef CHECK_AND_RETURN_WITHCONFIG
}
// parses the model name only. The configuration is ignored. Even if it's
// missing or unparseable. Unknown model names return MRCAL_LENSMODEL_INVALID
mrcal_lensmodel_type_t mrcal_lensmodel_type_from_name( const char* name )
{
#define CHECK_AND_RETURN_NOCONFIG(s,n) \
if( 0 == strcmp( name, #s) ) return MRCAL_##s;
#define CHECK_AND_RETURN_WITHCONFIG(s,n) \
/* Configured model. If the name is followed by _ or nothing, I */ \
/* accept this model */ \
if( 0 == strcmp( name, #s) ) return MRCAL_##s; \
if( 0 == strncmp( name, #s"_", strlen(#s)+1) ) return MRCAL_##s;
MRCAL_LENSMODEL_NOCONFIG_LIST( CHECK_AND_RETURN_NOCONFIG );
MRCAL_LENSMODEL_WITHCONFIG_LIST( CHECK_AND_RETURN_WITHCONFIG );
return MRCAL_LENSMODEL_INVALID;
#undef CHECK_AND_RETURN_NOCONFIG
#undef CHECK_AND_RETURN_WITHCONFIG
}
mrcal_lensmodel_metadata_t mrcal_lensmodel_metadata( const mrcal_lensmodel_t m )
{
switch(m.type)
{
case MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC:
case MRCAL_LENSMODEL_STEREOGRAPHIC:
return (mrcal_lensmodel_metadata_t) { .has_core = true,
.can_project_behind_camera = true };
case MRCAL_LENSMODEL_PINHOLE:
case MRCAL_LENSMODEL_OPENCV4:
case MRCAL_LENSMODEL_OPENCV5:
case MRCAL_LENSMODEL_OPENCV8:
case MRCAL_LENSMODEL_OPENCV12:
case MRCAL_LENSMODEL_CAHVOR:
case MRCAL_LENSMODEL_CAHVORE:
return (mrcal_lensmodel_metadata_t) { .has_core = true,
.can_project_behind_camera = false };
default: ;
}
MSG("Unknown lens model %d. Barfing out", m.type);
assert(0);
}
static
bool modelHasCore_fxfycxcy( const mrcal_lensmodel_t m )
{
mrcal_lensmodel_metadata_t meta = mrcal_lensmodel_metadata(m);
return meta.has_core;
}
static
bool model_supports_projection_behind_camera( const mrcal_lensmodel_t m )
{
mrcal_lensmodel_metadata_t meta = mrcal_lensmodel_metadata(m);
return meta.can_project_behind_camera;
}
static int LENSMODEL_SPLINED_STEREOGRAPHIC__lensmodel_num_params(const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config)
{
return
// I have two surfaces: one for x and another for y
(int)config->Nx * (int)config->Ny * 2 +
// and I have a core
4;
}
int mrcal_lensmodel_num_params(const mrcal_lensmodel_t m)
{
switch(m.type)
{
#define CASE_NUM_NOCONFIG(s,n) \
case MRCAL_##s: return n;
#define CASE_NUM_WITHCONFIG(s,n) \
case MRCAL_##s: return s##__lensmodel_num_params(&m.s##__config);
MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_NUM_NOCONFIG )
MRCAL_LENSMODEL_WITHCONFIG_LIST( CASE_NUM_WITHCONFIG )
default: ;
}
return -1;
#undef CASE_NUM_NOCONFIG
#undef CASE_NUM_WITHCONFIG
}
static
int get_num_distortions_optimization_params(mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
if( !problem_details.do_optimize_intrinsics_distortions )
return 0;
int N = mrcal_lensmodel_num_params(lensmodel);
if(modelHasCore_fxfycxcy(lensmodel))
N -= 4; // ignoring fx,fy,cx,cy
return N;
}
int mrcal_num_intrinsics_optimization_params(mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
int N = get_num_distortions_optimization_params(problem_details, lensmodel);
if( problem_details.do_optimize_intrinsics_core &&
modelHasCore_fxfycxcy(lensmodel) )
N += 4; // fx,fy,cx,cy
return N;
}
int mrcal_num_states(int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return
mrcal_num_states_intrinsics(Ncameras_intrinsics,
problem_details,
lensmodel) +
mrcal_num_states_extrinsics(Ncameras_extrinsics,
problem_details) +
mrcal_num_states_frames(Nframes,
problem_details) +
mrcal_num_states_points(Npoints, Npoints_fixed,
problem_details) +
mrcal_num_states_calobject_warp( problem_details,
Nobservations_board);
}
static int num_regularization_terms_percamera(mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
if(!problem_details.do_apply_regularization)
return 0;
// distortions
int N = get_num_distortions_optimization_params(problem_details, lensmodel);
// optical center
if(problem_details.do_optimize_intrinsics_core)
N += 2;
return N;
}
int mrcal_measurement_index_boards(int i_observation_board,
int Nobservations_board,
int Nobservations_point,
int calibration_object_width_n,
int calibration_object_height_n)
{
// *2 because I have separate x and y measurements
return
0 +
i_observation_board *
calibration_object_width_n*calibration_object_height_n *
2;
}
int mrcal_num_measurements_boards(int Nobservations_board,
int calibration_object_width_n,
int calibration_object_height_n)
{
return mrcal_measurement_index_boards( Nobservations_board,
0,0,
calibration_object_width_n,
calibration_object_height_n);
}
int mrcal_measurement_index_points(int i_observation_point,
int Nobservations_board,
int Nobservations_point,
int calibration_object_width_n,
int calibration_object_height_n)
{
// 3: x,y measurements, range normalization
return
mrcal_num_measurements_boards(Nobservations_board,
calibration_object_width_n,
calibration_object_height_n) +
i_observation_point * 3;
}
int mrcal_num_measurements_points(int Nobservations_point)
{
// 3: x,y measurements, range normalization
return Nobservations_point * 3;
}
int mrcal_measurement_index_regularization(int Nobservations_board,
int Nobservations_point,
int calibration_object_width_n,
int calibration_object_height_n)
{
return
mrcal_num_measurements_boards(Nobservations_board,
calibration_object_width_n,
calibration_object_height_n) +
mrcal_num_measurements_points(Nobservations_point);
}
int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return
Ncameras_intrinsics *
num_regularization_terms_percamera(problem_details, lensmodel);
}
int mrcal_num_measurements(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,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return
mrcal_num_measurements_boards( Nobservations_board,
calibration_object_width_n,
calibration_object_height_n) +
mrcal_num_measurements_points(Nobservations_point) +
mrcal_num_measurements_regularization(Ncameras_intrinsics, Ncameras_extrinsics,
Nframes,
Npoints, Npoints_fixed, Nobservations_board,
problem_details,
lensmodel);
}
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,
const mrcal_observation_board_t* observations_board,
const mrcal_observation_point_t* observations_point,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
// each observation depends on all the parameters for THAT frame and for
// THAT camera. Camera0 doesn't have extrinsics, so I need to loop through
// all my observations
// Each projected point has an x and y measurement, and each one depends on
// some number of the intrinsic parameters. Parametric models are simple:
// each one depends on ALL of the intrinsics. Splined models are sparse,
// however, and there's only a partial dependence
int Nintrinsics_per_measurement;
if(lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC)
{
int run_len =
lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order + 1;
Nintrinsics_per_measurement =
(problem_details.do_optimize_intrinsics_core ? 4 : 0) +
(problem_details.do_optimize_intrinsics_distortions ? (run_len*run_len) : 0);
}
else
Nintrinsics_per_measurement =
mrcal_num_intrinsics_optimization_params(problem_details, lensmodel);
// x depends on fx,cx but NOT on fy, cy. And similarly for y.
if( problem_details.do_optimize_intrinsics_core &&
modelHasCore_fxfycxcy(lensmodel) )
Nintrinsics_per_measurement -= 2;
int N = Nobservations_board * ( (problem_details.do_optimize_frames ? 6 : 0) +
(problem_details.do_optimize_extrinsics ? 6 : 0) +
(problem_details.do_optimize_calobject_warp ? 2 : 0) +
Nintrinsics_per_measurement );
// initial estimate counts extrinsics for the reference camera, which need
// to be subtracted off
if(problem_details.do_optimize_extrinsics)
for(int i=0; i<Nobservations_board; i++)
if(observations_board[i].icam.extrinsics < 0)
N -= 6;
// *2 because I have separate x and y measurements
N *= 2*calibration_object_width_n*calibration_object_height_n;
// Now the point observations
for(int i=0; i<Nobservations_point; i++)
{
N += 2*Nintrinsics_per_measurement;
if( problem_details.do_optimize_frames &&
observations_point[i].i_point < Npoints-Npoints_fixed )
N += 2*3;
if( problem_details.do_optimize_extrinsics &&
observations_point[i].icam.extrinsics >= 0 )
N += 2*6;
// range normalization
if(problem_details.do_optimize_frames &&
observations_point[i].i_point < Npoints-Npoints_fixed )
N += 3;
if( problem_details.do_optimize_extrinsics &&
observations_point[i].icam.extrinsics >= 0 )
N += 6;
}
N +=
Ncameras_intrinsics *
num_regularization_terms_percamera(problem_details,
lensmodel);
return N;
}
// Used in the spline-based projection function.
//
// See bsplines.py for the derivation of the spline expressions and for
// justification of the 2D scheme
//
// Here we sample two interpolated surfaces at once: one each for the x and y
// focal-length scales
static
void sample_bspline_surface_cubic(double* out,
double* dout_dx, // may be NULL
double* dout_dy, // may be NULL
double* ABCDx_ABCDy,
double x, double y,
// control points
const double* c,
int stridey
// stridex is 2: the control points from the
// two surfaces are next to each other. Better
// cache locality maybe
)
{
double* ABCDx = &ABCDx_ABCDy[0];
double* ABCDy = &ABCDx_ABCDy[4];
// The sampling function assumes evenly spaced knots.
// a,b,c,d are sequential control points
// x is in [0,1] between b and c. Function looks like this:
// double A = fA(x);
// double B = fB(x);
// double C = fC(x);
// double D = fD(x);
// return A*a + B*b + C*c + D*d;
// I need to sample many such 1D segments, so I compute A,B,C,D separately,
// and apply them together
void get_sample_coeffs(double* ABCD, double* ABCDgrad, double x)
{
double x2 = x*x;
double x3 = x2*x;
ABCD[0] = (-x3 + 3*x2 - 3*x + 1)/6;
ABCD[1] = (3 * x3/2 - 3*x2 + 2)/3;
ABCD[2] = (-3 * x3 + 3*x2 + 3*x + 1)/6;
ABCD[3] = x3 / 6;
ABCDgrad[0] = -x2/2 + x - 1./2.;
ABCDgrad[1] = 3*x2/2 - 2*x;
ABCDgrad[2] = -3*x2/2 + x + 1./2.;
ABCDgrad[3] = x2 / 2;
}
// 4 samples along one dimension, and then one sample along the other
// dimension, using the 4 samples as the control points. Order doesn't
// matter. See bsplines.py
//
// I do this twice: one for each focal length surface
double ABCDgradx[4];
double ABCDgrady[4];
get_sample_coeffs(ABCDx, ABCDgradx, x);
get_sample_coeffs(ABCDy, ABCDgrady, y);
void interp(double* out, const double* ABCDx, const double* ABCDy)
{
double cinterp[4][2];
const int stridex = 2;
for(int iy=0; iy<4; iy++)
for(int k=0;k<2;k++)
cinterp[iy][k] =
ABCDx[0] * c[iy*stridey + 0*stridex + k] +
ABCDx[1] * c[iy*stridey + 1*stridex + k] +
ABCDx[2] * c[iy*stridey + 2*stridex + k] +
ABCDx[3] * c[iy*stridey + 3*stridex + k];
for(int k=0;k<2;k++)
out[k] =
ABCDy[0] * cinterp[0][k] +
ABCDy[1] * cinterp[1][k] +
ABCDy[2] * cinterp[2][k] +
ABCDy[3] * cinterp[3][k];
}
// the intrinsics gradient is flatten(ABCDx[0..3] * ABCDy[0..3]) for both x
// and y. By returning ABCD[xy] and not the cartesian products, I make
// smaller temporary data arrays
interp(out, ABCDx, ABCDy);
if(dout_dx)
interp(dout_dx, ABCDgradx, ABCDy);
if(dout_dy)
interp(dout_dy, ABCDx, ABCDgrady);
}
static
void sample_bspline_surface_quadratic(double* out,
double* dout_dx, // may be NULL
double* dout_dy, // may be NULL
double* ABCx_ABCy,
double x, double y,
// control points
const double* c,
int stridey
// stridex is 2: the control points from the
// two surfaces are next to each other. Better
// cache locality maybe
)
{
double* ABCx = &ABCx_ABCy[0];
double* ABCy = &ABCx_ABCy[3];
// The sampling function assumes evenly spaced knots.
// a,b,c are sequential control points
// x is in [-1/2,1/2] around b. Function looks like this:
// double A = fA(x);
// double B = fB(x);
// double C = fC(x);
// return A*a + B*b + C*c;
// I need to sample many such 1D segments, so I compute A,B,C separately,
// and apply them together
void get_sample_coeffs(double* ABC, double* ABCgrad, double x)
{
double x2 = x*x;
ABC[0] = (4*x2 - 4*x + 1)/8;
ABC[1] = (3 - 4*x2)/4;
ABC[2] = (4*x2 + 4*x + 1)/8;
ABCgrad[0] = x - 1./2.;
ABCgrad[1] = -2.*x;
ABCgrad[2] = x + 1./2.;
}
// 3 samples along one dimension, and then one sample along the other
// dimension, using the 3 samples as the control points. Order doesn't
// matter. See bsplines.py
//
// I do this twice: one for each focal length surface
double ABCgradx[3];
double ABCgrady[3];
get_sample_coeffs(ABCx, ABCgradx, x);
get_sample_coeffs(ABCy, ABCgrady, y);
void interp(double* out, const double* ABCx, const double* ABCy)
{
double cinterp[3][2];
const int stridex = 2;
for(int iy=0; iy<3; iy++)
for(int k=0;k<2;k++)
cinterp[iy][k] =
ABCx[0] * c[iy*stridey + 0*stridex + k] +
ABCx[1] * c[iy*stridey + 1*stridex + k] +
ABCx[2] * c[iy*stridey + 2*stridex + k];
for(int k=0;k<2;k++)
out[k] =
ABCy[0] * cinterp[0][k] +
ABCy[1] * cinterp[1][k] +
ABCy[2] * cinterp[2][k];
}
// the intrinsics gradient is flatten(ABCx[0..3] * ABCy[0..3]) for both x
// and y. By returning ABC[xy] and not the cartesian products, I make
// smaller temporary data arrays
interp(out, ABCx, ABCy);
if(dout_dx)
interp(dout_dx, ABCgradx, ABCy);
if(dout_dy)
interp(dout_dy, ABCx, ABCgrady);
}
typedef struct
{
double _d_rj_rf[3*3];
double _d_rj_rc[3*3];
double _d_tj_tf[3*3];
double _d_tj_rc[3*3];
// _d_tj_tc is always identity
// _d_tj_rf is always 0
// _d_rj_tf is always 0
// _d_rj_tc is always 0
} geometric_gradients_t;
// The implementation of _mrcal_project_internal_opencv is based on opencv. The
// sources have been heavily modified, but the opencv logic remains. This
// function is a cut-down cvProjectPoints2Internal() to keep only the
// functionality I want and to use my interfaces. Putting this here allows me to
// drop the C dependency on opencv. Which is a good thing, since opencv dropped
// their C API
//
// from opencv-4.2.0+dfsg/modules/calib3d/src/calibration.cpp
//
// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
// Third party copyrights are property of their respective owners.
//
// Redistribution and use in source and binary forms, with or without modification,
// are permitted provided that the following conditions are met:
//
// * Redistribution's of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
//
// * Redistribution's in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
//
// * The name of the copyright holders may not be used to endorse or promote products
// derived from this software without specific prior written permission.
//
// This software is provided by the copyright holders and contributors "as is" and
// any express or implied warranties, including, but not limited to, the implied
// warranties of merchantability and fitness for a particular purpose are disclaimed.
// In no event shall the Intel Corporation or contributors be liable for any direct,
// indirect, incidental, special, exemplary, or consequential damages
// (including, but not limited to, procurement of substitute goods or services;
// loss of use, data, or profits; or business interruption) however caused
// and on any theory of liability, whether in contract, strict liability,
// or tort (including negligence or otherwise) arising in any way out of
// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper
// only
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)
{
const double fx = intrinsics[0];
const double fy = intrinsics[1];
const double cx = intrinsics[2];
const double cy = intrinsics[3];
double k[12] = {};
for(int i=0; i<Nintrinsics-4; i++)
k[i] = intrinsics[i+4];
for( int i = 0; i < N; i++ )
{
double z_recip = 1./p[i].z;
double x = p[i].x * z_recip;
double y = p[i].y * z_recip;
double r2 = x*x + y*y;
double r4 = r2*r2;
double r6 = r4*r2;
double a1 = 2*x*y;
double a2 = r2 + 2*x*x;
double a3 = r2 + 2*y*y;
double cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
double icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
double xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
double yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
q[i].x = xd*fx + cx;
q[i].y = yd*fy + cy;
if( dq_dp )
{
double dx_dp[] = { z_recip, 0, -x*z_recip };
double dy_dp[] = { 0, z_recip, -y*z_recip };
for( int j = 0; j < 3; j++ )
{
double dr2_dp = 2*x*dx_dp[j] + 2*y*dy_dp[j];
double dcdist_dp = k[0]*dr2_dp + 2*k[1]*r2*dr2_dp + 3*k[4]*r4*dr2_dp;
double dicdist2_dp = -icdist2*icdist2*(k[5]*dr2_dp + 2*k[6]*r2*dr2_dp + 3*k[7]*r4*dr2_dp);
double da1_dp = 2*(x*dy_dp[j] + y*dx_dp[j]);
double dmx_dp = (dx_dp[j]*cdist*icdist2 + x*dcdist_dp*icdist2 + x*cdist*dicdist2_dp +
k[2]*da1_dp + k[3]*(dr2_dp + 4*x*dx_dp[j]) + k[8]*dr2_dp + 2*r2*k[9]*dr2_dp);
double dmy_dp = (dy_dp[j]*cdist*icdist2 + y*dcdist_dp*icdist2 + y*cdist*dicdist2_dp +
k[2]*(dr2_dp + 4*y*dy_dp[j]) + k[3]*da1_dp + k[10]*dr2_dp + 2*r2*k[11]*dr2_dp);
dq_dp[i*2 + 0].xyz[j] = fx*dmx_dp;
dq_dp[i*2 + 1].xyz[j] = fy*dmy_dp;
}
}
if( dq_dintrinsics_nocore )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 0] = fx*x*icdist2*r2;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 0] = fy*(y*icdist2*r2);
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 1] = fx*x*icdist2*r4;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 1] = fy*y*icdist2*r4;
if( Nintrinsics-4 > 2 )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 2] = fx*a1;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 2] = fy*a3;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 3] = fx*a2;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 3] = fy*a1;
if( Nintrinsics-4 > 4 )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 4] = fx*x*icdist2*r6;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 4] = fy*y*icdist2*r6;
if( Nintrinsics-4 > 5 )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 5] = fx*x*cdist*(-icdist2)*icdist2*r2;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 5] = fy*y*cdist*(-icdist2)*icdist2*r2;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 6] = fx*x*cdist*(-icdist2)*icdist2*r4;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 6] = fy*y*cdist*(-icdist2)*icdist2*r4;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 7] = fx*x*cdist*(-icdist2)*icdist2*r6;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 7] = fy*y*cdist*(-icdist2)*icdist2*r6;
if( Nintrinsics-4 > 8 )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 8] = fx*r2; //s1
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 8] = fy*0; //s1
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 9] = fx*r4; //s2
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 9] = fy*0; //s2
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 10] = fx*0;//s3
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 10] = fy*r2; //s3
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 11] = fx*0;//s4
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 11] = fy*r4; //s4
}
}
}
}
}
}
}
// These are all internals for project(). It was getting unwieldy otherwise
static
void _project_point_parametric( // outputs
mrcal_point2_t* q,
mrcal_point2_t* dq_dfxy, double* dq_dintrinsics_nocore,
mrcal_point3_t* restrict dq_drcamera,
mrcal_point3_t* restrict dq_dtcamera,
mrcal_point3_t* restrict dq_drframe,
mrcal_point3_t* restrict dq_dtframe,
// inputs
const mrcal_point3_t* p,
const mrcal_point3_t* dp_drc,
const mrcal_point3_t* dp_dtc,
const mrcal_point3_t* dp_drf,
const mrcal_point3_t* dp_dtf,
const double* restrict intrinsics,
bool camera_at_identity,
mrcal_lensmodel_t lensmodel)
{
// u = distort(p, distortions)
// q = uxy/uz * fxy + cxy
if( lensmodel.type == MRCAL_LENSMODEL_PINHOLE ||
lensmodel.type == MRCAL_LENSMODEL_STEREOGRAPHIC ||
MRCAL_LENSMODEL_IS_OPENCV(lensmodel.type) )
{
// q = fxy pxy/pz + cxy
// dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1])
// dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1])
const double fx = intrinsics[0];
const double fy = intrinsics[1];
const double cx = intrinsics[2];
const double cy = intrinsics[3];
mrcal_point3_t dq_dp[2];
if( lensmodel.type == MRCAL_LENSMODEL_PINHOLE )
{
double pz_recip = 1. / p->z;
q->x = p->x*pz_recip * fx + cx;
q->y = p->y*pz_recip * fy + cy;
dq_dp[0].x = fx * pz_recip;
dq_dp[0].y = 0;
dq_dp[0].z = -fx*p->x*pz_recip*pz_recip;
dq_dp[1].x = 0;
dq_dp[1].y = fy * pz_recip;
dq_dp[1].z = -fy*p->y*pz_recip*pz_recip;
}
else if(lensmodel.type == MRCAL_LENSMODEL_STEREOGRAPHIC)
{
mrcal_project_stereographic(q, dq_dp,
p, 1, fx,fy,cx,cy);
}
else
{
int Nintrinsics = mrcal_lensmodel_num_params(lensmodel);
_mrcal_project_internal_opencv( q, dq_dp,
dq_dintrinsics_nocore,
p, 1, intrinsics, Nintrinsics);
}
// dq/deee = dq/dp dp/deee
if(camera_at_identity)
{
if( dq_drcamera != NULL ) memset(dq_drcamera, 0, 6*sizeof(double));
if( dq_dtcamera != NULL ) memset(dq_dtcamera, 0, 6*sizeof(double));
if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drf, (double*)dq_drframe);
if( dq_dtframe != NULL ) memcpy(dq_dtframe, (double*)dq_dp, 6*sizeof(double));
}
else
{
if( dq_drcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drc, (double*)dq_drcamera);
if( dq_dtcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_dtc, (double*)dq_dtcamera);
if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drf, (double*)dq_drframe );
if( dq_dtframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_dtf, (double*)dq_dtframe );
}
// I have the projection, and I now need to propagate the gradients
if( dq_dfxy )
{
// I have the projection, and I now need to propagate the gradients
// xy = fxy * distort(xy)/distort(z) + cxy
dq_dfxy->x = (q->x - cx)/fx; // dqx/dfx
dq_dfxy->y = (q->y - cy)/fy; // dqy/dfy
}
}
else if( lensmodel.type == MRCAL_LENSMODEL_CAHVOR )
{
int NdistortionParams = mrcal_lensmodel_num_params(lensmodel) - 4;
// I perturb p, and then apply the focal length, center pixel stuff
// normally
mrcal_point3_t p_distorted;
// distortion parameter layout:
// alpha
// beta
// r0
// r1
// r2
double alpha = intrinsics[4 + 0];
double beta = intrinsics[4 + 1];
double r0 = intrinsics[4 + 2];
double r1 = intrinsics[4 + 3];
double r2 = intrinsics[4 + 4];
double s_al, c_al, s_be, c_be;
sincos(alpha, &s_al, &c_al);
sincos(beta, &s_be, &c_be);
// I parametrize the optical axis such that
// - o(alpha=0, beta=0) = (0,0,1) i.e. the optical axis is at the center
// if both parameters are 0
// - The gradients are cartesian. I.e. do/dalpha and do/dbeta are both
// NOT 0 at (alpha=0,beta=0). This would happen at the poles (gimbal
// lock), and that would make my solver unhappy
double o [] = { s_al*c_be, s_be, c_al*c_be };
double do_dalpha[] = { c_al*c_be, 0, -s_al*c_be };
double do_dbeta[] = { -s_al*s_be, c_be, -c_al*s_be };
double norm2p = norm2_vec(3, p->xyz);
double omega = dot_vec(3, p->xyz, o);
double domega_dalpha = dot_vec(3, p->xyz, do_dalpha);
double domega_dbeta = dot_vec(3, p->xyz, do_dbeta);
double omega_recip = 1.0 / omega;
double tau = norm2p * omega_recip*omega_recip - 1.0;
double s__dtau_dalphabeta__domega_dalphabeta = -2.0*norm2p * omega_recip*omega_recip*omega_recip;
double dmu_dtau = r1 + 2.0*tau*r2;
double dmu_dxyz[3];
for(int i=0; i<3; i++)
dmu_dxyz[i] = dmu_dtau *
(2.0 * p->xyz[i] * omega_recip*omega_recip + s__dtau_dalphabeta__domega_dalphabeta * o[i]);
double mu = r0 + tau*r1 + tau*tau*r2;
double s__dmu_dalphabeta__domega_dalphabeta = dmu_dtau * s__dtau_dalphabeta__domega_dalphabeta;
double dpdistorted_dpcam[3*3] = {};
double dpdistorted_ddistortion[3*NdistortionParams];
for(int i=0; i<3; i++)
{
double dmu_ddist[5] = { s__dmu_dalphabeta__domega_dalphabeta * domega_dalpha,
s__dmu_dalphabeta__domega_dalphabeta * domega_dbeta,
1.0,
tau,
tau * tau };
dpdistorted_ddistortion[i*NdistortionParams + 0] = p->xyz[i] * dmu_ddist[0];
dpdistorted_ddistortion[i*NdistortionParams + 1] = p->xyz[i] * dmu_ddist[1];
dpdistorted_ddistortion[i*NdistortionParams + 2] = p->xyz[i] * dmu_ddist[2];
dpdistorted_ddistortion[i*NdistortionParams + 3] = p->xyz[i] * dmu_ddist[3];
dpdistorted_ddistortion[i*NdistortionParams + 4] = p->xyz[i] * dmu_ddist[4];
dpdistorted_ddistortion[i*NdistortionParams + 0] -= dmu_ddist[0] * omega*o[i];
dpdistorted_ddistortion[i*NdistortionParams + 1] -= dmu_ddist[1] * omega*o[i];
dpdistorted_ddistortion[i*NdistortionParams + 2] -= dmu_ddist[2] * omega*o[i];
dpdistorted_ddistortion[i*NdistortionParams + 3] -= dmu_ddist[3] * omega*o[i];
dpdistorted_ddistortion[i*NdistortionParams + 4] -= dmu_ddist[4] * omega*o[i];
dpdistorted_ddistortion[i*NdistortionParams + 0] -= mu * domega_dalpha*o[i];
dpdistorted_ddistortion[i*NdistortionParams + 1] -= mu * domega_dbeta *o[i];
dpdistorted_ddistortion[i*NdistortionParams + 0] -= mu * omega * do_dalpha[i];
dpdistorted_ddistortion[i*NdistortionParams + 1] -= mu * omega * do_dbeta [i];
dpdistorted_dpcam[3*i + i] = mu+1.0;
for(int j=0; j<3; j++)
{
dpdistorted_dpcam[3*i + j] += (p->xyz[i] - omega*o[i]) * dmu_dxyz[j];
dpdistorted_dpcam[3*i + j] -= mu*o[i]*o[j];
}
p_distorted.xyz[i] = p->xyz[i] + mu * (p->xyz[i] - omega*o[i]);
}
// q = fxy pxy/pz + cxy
// dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1])
// dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1])
const double fx = intrinsics[0];
const double fy = intrinsics[1];
const double cx = intrinsics[2];
const double cy = intrinsics[3];
double pz_recip = 1. / p_distorted.z;
q->x = p_distorted.x*pz_recip * fx + cx;
q->y = p_distorted.y*pz_recip * fy + cy;
double dq_dp[2][3] =
{ { fx * pz_recip, 0, -fx*p_distorted.x*pz_recip*pz_recip},
{ 0, fy * pz_recip, -fy*p_distorted.y*pz_recip*pz_recip} };
// This is for the DISTORTED p.
// dq/deee = dq/dpdistorted dpdistorted/dpundistorted dpundistorted/deee
double dq_dpundistorted[6];
mul_genN3_gen33_vout(2, (double*)dq_dp, dpdistorted_dpcam, dq_dpundistorted);
// dq/deee = dq/dp dp/deee
if(camera_at_identity)
{
if( dq_drcamera != NULL ) memset(dq_drcamera, 0, 6*sizeof(double));
if( dq_dtcamera != NULL ) memset(dq_dtcamera, 0, 6*sizeof(double));
if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe);
if( dq_dtframe != NULL ) memcpy(dq_dtframe, dq_dpundistorted, 6*sizeof(double));
}
else
{
if( dq_drcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drc, (double*)dq_drcamera);
if( dq_dtcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtc, (double*)dq_dtcamera);
if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe );
if( dq_dtframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtf, (double*)dq_dtframe );
}
if( dq_dintrinsics_nocore != NULL )
{
for(int i=0; i<NdistortionParams; i++)
{
const double dx = dpdistorted_ddistortion[i + 0*NdistortionParams];
const double dy = dpdistorted_ddistortion[i + 1*NdistortionParams];
const double dz = dpdistorted_ddistortion[i + 2*NdistortionParams];
dq_dintrinsics_nocore[0*NdistortionParams + i] = fx * pz_recip * (dx - p_distorted.x*pz_recip*dz);
dq_dintrinsics_nocore[1*NdistortionParams + i] = fy * pz_recip * (dy - p_distorted.y*pz_recip*dz);
}
}
if( dq_dfxy )
{
// I have the projection, and I now need to propagate the gradients
// xy = fxy * distort(xy)/distort(z) + cxy
dq_dfxy->x = p_distorted.x*pz_recip; // dx/dfx
dq_dfxy->y = p_distorted.y*pz_recip; // dy/dfy
}
}
else
{
MSG("Unhandled lens model: %d (%s)",
lensmodel.type, mrcal_lensmodel_name_unconfigured(lensmodel));
assert(0);
}
}
// Compute a stereographic projection using a constant fxy, cxy. This is the
// same as the pinhole projection for long lenses, but supports views behind the
// camera
void mrcal_project_stereographic( // output
mrcal_point2_t* q,
mrcal_point3_t* dq_dv, // May be NULL. Each point
// gets a block of 2 mrcal_point3_t
// objects
// input
const mrcal_point3_t* v,
int N,
double fx, double fy,
double cx, double cy)
{
// stereographic projection:
// (from https://en.wikipedia.org/wiki/Fisheye_lens)
// u = xy_unit * tan(th/2) * 2
//
// I compute the normalized (focal-length = 1) projection, and
// use that to look-up the x and y focal length scalings
// th is the angle between the observation and the projection
// center
//
// sin(th) = mag_xy/mag_xyz
// cos(th) = z/mag_xyz
// tan(th/2) = sin(th) / (1 + cos(th))
// tan(th/2) = mag_xy/mag_xyz / (1 + z/mag_xyz) =
// = mag_xy / (mag_xyz + z)
// u = xy_unit * tan(th/2) * 2 =
// = xy/mag_xy * mag_xy/(mag_xyz + z) * 2 =
// = xy / (mag_xyz + z) * 2
for(int i=0; i<N; i++)
{
double mag_xyz = sqrt( v[i].x*v[i].x +
v[i].y*v[i].y +
v[i].z*v[i].z );
double scale = 2.0 / (mag_xyz + v[i].z);
if(dq_dv)
{
// this is more or less already derived in _project_point_splined()
//
// dqx/dv = fx ( scale dx + x dscale ) =
// = fx ( [1 0 0] scale - 2 x / ()^2 * ( [x y z]/(sqrt) + [0 0 1]) )
// = fx ( [scale 0 0] - x scale^2/2 * ( [x y z]/mag_xyz + [0 0 1]) )
// Let A = -scale^2/2
// B = A/mag_xyz
// dqx_dv = fx ( [scale 0 0] - x scale^2/2 * [x y z]/mag_xyz - x scale^2/2 [0 0 1] )
// = fx ( [scale 0 0] + B x * [x y z] + x A [0 0 1] )
double A = -scale*scale / 2.;
double B = A / mag_xyz;
dq_dv[2*i + 0] = (mrcal_point3_t){.x = fx * (v[i].x * (B*v[i].x) + scale),
.y = fx * (v[i].x * (B*v[i].y)),
.z = fx * (v[i].x * (B*v[i].z + A))};
dq_dv[2*i + 1] = (mrcal_point3_t){.x = fy * (v[i].y * (B*v[i].x)),
.y = fy * (v[i].y * (B*v[i].y) + scale),
.z = fy * (v[i].y * (B*v[i].z + A))};
}
q[i] = (mrcal_point2_t){.x = v[i].x * scale * fx + cx,
.y = v[i].y * scale * fy + cy};
}
}
// Compute a stereographic unprojection using a constant fxy, cxy
void mrcal_unproject_stereographic( // output
mrcal_point3_t* v,
mrcal_point2_t* dv_dq, // May be NULL. Each point
// gets a block of 3
// mrcal_point2_t objects
// input
const mrcal_point2_t* q,
int N,
double fx, double fy,
double cx, double cy)
{
// stereographic projection:
// (from https://en.wikipedia.org/wiki/Fisheye_lens)
// u = xy_unit * tan(th/2) * 2
//
// I compute the normalized (focal-length = 1) projection, and
// use that to look-up the x and y focal length scalings
//
// th is the angle between the observation and the projection
// center
//
// sin(th) = mag_xy/mag_xyz
// cos(th) = z/mag_xyz
// tan(th/2) = sin(th) / (1 + cos(th))
//
// tan(th/2) = mag_xy/mag_xyz / (1 + z/mag_xyz) =
// = mag_xy / (mag_xyz + z)
// u = xy_unit * tan(th/2) * 2 =
// = xy/mag_xy * mag_xy/(mag_xyz + z) * 2 =
// = xy / (mag_xyz + z) * 2
//
// How do I compute the inverse?
//
// So q = u f + c
// -> u = (q-c)/f
// mag(u) = tan(th/2)*2
//
// So I can compute th. az comes from the direction of u. This is enough to
// compute everything. th is in [0,pi].
//
// [ sin(th) cos(az) ] [ cos(az) ]
// v = [ sin(th) sin(az) ] ~ [ sin(az) ]
// [ cos(th) ] [ 1/tan(th) ]
//
// mag(u) = tan(th/2)*2 -> mag(u)/2 = tan(th/2) ->
// tan(th) = mag(u) / (1 - (mag(u)/2)^2)
// 1/tan(th) = (1 - 1/4*mag(u)^2) / mag(u)
//
// This has a singularity at u=0 (imager center). But I can scale v to avoid
// this. So
//
// [ cos(az) mag(u) ]
// v = [ sin(az) mag(u) ]
// [ 1 - 1/4*mag(u)^2 ]
//
// I can simplify this even more. az = atan2(u.y,u.x). cos(az) = u.x/mag(u) ->
//
// [ u.x ]
// v = [ u.y ]
// [ 1 - 1/4 mag(u)^2 ]
//
// Test script to confirm that the project/unproject expressions are
// correct. unproj(proj(v))/v should be a constant
//
// import numpy as np
// import numpysane as nps
// f = 2000
// c = 1000
// def proj(v):
// m = nps.mag(v)
// scale = 2.0 / (m + v[..., 2])
// u = v[..., :2] * nps.dummy(scale, -1)
// return u * f + c
// def unproj(q):
// u = (q-c)/f
// muxy = nps.mag(u[..., :2])
// m = nps.mag(u)
// return nps.mv(nps.cat( u[..., 0],
// u[..., 1],
// 1 - 1./4.* m*m),
// 0, -1)
// v = np.array(((1., 2., 3.),
// (3., -2., -4.)))
// print( unproj(proj(v)) / v)
for(int i=0; i<N; i++)
{
mrcal_point2_t u = {.x = (q[i].x - cx) / fx,
.y = (q[i].y - cy) / fy};
double norm2u = u.x*u.x + u.y*u.y;
if(dv_dq)
{
dv_dq[3*i + 0] = (mrcal_point2_t){.x = 1.0/fx};
dv_dq[3*i + 1] = (mrcal_point2_t){.y = 1.0/fy};
dv_dq[3*i + 2] = (mrcal_point2_t){.x = -u.x/2.0/fx,
.y = -u.y/2.0/fy};
}
v[i] = (mrcal_point3_t){ .x = u.x,
.y = u.y,
.z = 1. - 1./4. * norm2u };
}
}
static void _mrcal_precompute_lensmodel_data_MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC
( // output
mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t* precomputed,
//input
const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config )
{
// I have N control points describing a given field-of-view. I
// want to space out the control points evenly. I'm using
// B-splines, so I need extra control points out past my edge.
// With cubic splines I need a whole extra interval past the
// edge. With quadratic splines I need half an interval (see
// stuff in analyses/splines/).
//
// (width + k*interval_size)/(N-1) = interval_size
// ---> width/(N-1) = interval_size * (1 - k/(N-1))
// ---> interval_size = width / (N - 1 - k)
int NextraIntervals;
if(config->order == 2)
{
NextraIntervals = 1;
if(config->Nx < 3 || config->Ny < 3)
{
MSG("Quadratic splines: absolute minimum Nx, Ny is 3. Got Nx=%d Ny=%d. Barfing out",
config->Nx, config->Ny);
assert(0);
}
}
else if(config->order == 3)
{
NextraIntervals = 2;
if(config->Nx < 4 || config->Ny < 4)
{
MSG("Cubic splines: absolute minimum Nx, Ny is 4. Got Nx=%d Ny=%d. Barfing out",
config->Nx, config->Ny);
assert(0);
}
}
else
{
MSG("I only support spline order 2 and 3");
assert(0);
}
double th_fov_x_edge = (double)config->fov_x_deg/2. * M_PI / 180.;
double u_edge_x = tan(th_fov_x_edge / 2.) * 2;
precomputed->segments_per_u = (config->Nx - 1 - NextraIntervals) / (u_edge_x*2.);
}
// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper
// only
void _mrcal_precompute_lensmodel_data(mrcal_projection_precomputed_t* precomputed,
mrcal_lensmodel_t lensmodel)
{
// currently only this model has anything
if(lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC)
_mrcal_precompute_lensmodel_data_MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC
( &precomputed->LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed,
&lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config );
precomputed->ready = true;
}
bool mrcal_knots_for_splined_models( // buffers must hold at least
// config->Nx and config->Ny values
// respectively
double* ux, double* uy,
mrcal_lensmodel_t lensmodel)
{
if(lensmodel.type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC)
{
MSG("This function works only with the MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC model. '%s' passed in",
mrcal_lensmodel_name_unconfigured(lensmodel));
return false;
}
mrcal_projection_precomputed_t precomputed_all;
_mrcal_precompute_lensmodel_data(&precomputed_all, lensmodel);
mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config =
&lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config;
mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t* precomputed =
&precomputed_all.LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed;
// The logic I'm reversing is
// double ix = u.x*segments_per_u + (double)(Nx-1)/2.;
for(int i=0; i<config->Nx; i++)
ux[i] =
((double)i - (double)(config->Nx-1)/2.) /
precomputed->segments_per_u;
for(int i=0; i<config->Ny; i++)
uy[i] =
((double)i - (double)(config->Ny-1)/2.) /
precomputed->segments_per_u;
return true;
}
static
void _project_point_splined( // outputs
mrcal_point2_t* q,
mrcal_point2_t* dq_dfxy,
double* grad_ABCDx_ABCDy,
int* ivar0,
// Gradient outputs. May be NULL
mrcal_point3_t* restrict dq_drcamera,
mrcal_point3_t* restrict dq_dtcamera,
mrcal_point3_t* restrict dq_drframe,
mrcal_point3_t* restrict dq_dtframe,
// inputs
const mrcal_point3_t* restrict p,
const mrcal_point3_t* restrict dp_drc,
const mrcal_point3_t* restrict dp_dtc,
const mrcal_point3_t* restrict dp_drf,
const mrcal_point3_t* restrict dp_dtf,
const double* restrict intrinsics,
bool camera_at_identity,
int spline_order,
uint16_t Nx, uint16_t Ny,
double segments_per_u)
{
// projections out-of-bounds will yield SOME value (function remains
// continuous as we move out-of-bounds), but it wont be particularly
// meaningful
// stereographic projection:
// (from https://en.wikipedia.org/wiki/Fisheye_lens)
// u = xy_unit * tan(th/2) * 2
//
// I compute the normalized (focal-length = 1) projection, and
// use that to look-up deltau
// th is the angle between the observation and the projection
// center
//
// sin(th) = mag_pxy/mag_p
// cos(th) = z/mag_p
// tan(th/2) = sin(th) / (1 + cos(th))
// tan(th/2) = mag_pxy/mag_p / (1 + z/mag_p) =
// = mag_pxy / (mag_p + z)
// u = xy_unit * tan(th/2) * 2 =
// = xy/mag_pxy * mag_pxy/(mag_p + z) * 2 =
// = xy / (mag_p + z) * 2
//
// The stereographic projection is used to query the spline surface, and it
// is also the projection baseline. I do
//
// q = (u + deltau(u)) * f + c
//
// If the spline surface is at 0 (deltau == 0) then this is a pure
// stereographic projection
double mag_p = sqrt( p->x*p->x +
p->y*p->y +
p->z*p->z );
double scale = 2.0 / (mag_p + p->z);
mrcal_point2_t u = {.x = p->x * scale,
.y = p->y * scale};
// du/dp = d/dp ( xy * scale )
// = pxy dscale/dp + [I; 0] scale
// dscale/dp = d (2.0 / (mag_p + p->z))/dp =
// = -2/()^2 ( [0,0,1] + dmag/dp)
// = -2/()^2 ( [0,0,1] + 2pt/2mag)
// = -2 scale^2/4 ( [0,0,1] + pt/mag)
// = -scale^2/2 * ( [0,0,1] + pt/mag )
// = A*[0,0,1] + B*pt
double A = -scale*scale / 2.;
double B = A / mag_p;
double du_dp[2][3] = { { p->x * (B * p->x) + scale,
p->x * (B * p->y),
p->x * (B * p->z + A) },
{ p->y * (B * p->x),
p->y * (B * p->y) + scale,
p->y * (B * p->z + A) } };
double ix = u.x*segments_per_u + (double)(Nx-1)/2.;
double iy = u.y*segments_per_u + (double)(Ny-1)/2.;
mrcal_point2_t deltau;
double ddeltau_dux[2];
double ddeltau_duy[2];
const double fx = intrinsics[0];
const double fy = intrinsics[1];
const double cx = intrinsics[2];
const double cy = intrinsics[3];
bool need_any_dq_drt =
!( dq_drcamera == NULL &&
dq_dtcamera == NULL &&
dq_drframe == NULL &&
dq_dtframe == NULL );
if( spline_order == 3 )
{
int ix0 = (int)ix;
int iy0 = (int)iy;
// If out-of-bounds, clamp to the nearest valid spline segment. The
// projection will fly off to infinity quickly (we're extrapolating a
// polynomial), but at least it'll stay continuous
if( ix0 < 1) ix0 = 1;
else if(ix0 > Nx-3) ix0 = Nx-3;
if( iy0 < 1) iy0 = 1;
else if(iy0 > Ny-3) iy0 = Ny-3;
*ivar0 =
4 + // skip the core
2*( (iy0-1)*Nx +
(ix0-1) );
sample_bspline_surface_cubic(deltau.xy,
need_any_dq_drt ? ddeltau_dux : NULL,
need_any_dq_drt ? ddeltau_duy : NULL,
grad_ABCDx_ABCDy,
ix - ix0, iy - iy0,
// control points
&intrinsics[*ivar0],
2*Nx);
}
else if( spline_order == 2 )
{
int ix0 = (int)(ix + 0.5);
int iy0 = (int)(iy + 0.5);
// If out-of-bounds, clamp to the nearest valid spline segment. The
// projection will fly off to infinity quickly (we're extrapolating a
// polynomial), but at least it'll stay continuous
if( ix0 < 1) ix0 = 1;
else if(ix0 > Nx-2) ix0 = Nx-2;
if( iy0 < 1) iy0 = 1;
else if(iy0 > Ny-2) iy0 = Ny-2;
*ivar0 =
4 + // skip the core
2*( (iy0-1)*Nx +
(ix0-1) );
sample_bspline_surface_quadratic(deltau.xy,
need_any_dq_drt ? ddeltau_dux : NULL,
need_any_dq_drt ? ddeltau_duy : NULL,
grad_ABCDx_ABCDy,
ix - ix0, iy - iy0,
// control points
&intrinsics[*ivar0],
2*Nx);
}
else
{
MSG("I only support spline order==2 or 3. Somehow got %d. This is a bug. Barfing",
spline_order);
assert(0);
}
// u = stereographic(p)
// q = (u + deltau(u)) * f + c
//
// Extrinsics:
// dqx/deee = fx (dux/deee + ddeltaux/du du/deee)
// = fx ( dux/deee (1 + ddeltaux/dux) + ddeltaux/duy duy/deee)
// dqy/deee = fy ( duy/deee (1 + ddeltauy/duy) + ddeltauy/dux dux/deee)
q->x = (u.x + deltau.x) * fx + cx;
q->y = (u.y + deltau.y) * fy + cy;
if( dq_dfxy )
{
// I have the projection, and I now need to propagate the gradients
// xy = fxy * distort(xy)/distort(z) + cxy
dq_dfxy->x = u.x + deltau.x;
dq_dfxy->y = u.y + deltau.y;
}
if(!need_any_dq_drt) return;
// convert ddeltau_dixy to ddeltau_duxy
for(int i=0; i<2; i++)
{
ddeltau_dux[i] *= segments_per_u;
ddeltau_duy[i] *= segments_per_u;
}
void propagate_extrinsics( mrcal_point3_t* dq_deee,
const mrcal_point3_t* dp_deee)
{
mrcal_point3_t du_deee[2];
mul_genN3_gen33_vout(2, (double*)du_dp, (double*)dp_deee, (double*)du_deee);
for(int i=0; i<3; i++)
{
dq_deee[0].xyz[i] =
fx *
( du_deee[0].xyz[i] * (1. + ddeltau_dux[0]) +
ddeltau_duy[0] * du_deee[1].xyz[i]);
dq_deee[1].xyz[i] =
fy *
( du_deee[1].xyz[i] * (1. + ddeltau_duy[1]) +
ddeltau_dux[1] * du_deee[0].xyz[i]);
}
}
void propagate_extrinsics_cam0( mrcal_point3_t* dq_deee)
{
for(int i=0; i<3; i++)
{
dq_deee[0].xyz[i] =
fx *
( du_dp[0][i] * (1. + ddeltau_dux[0]) +
ddeltau_duy[0] * du_dp[1][i]);
dq_deee[1].xyz[i] =
fy *
( du_dp[1][i] * (1. + ddeltau_duy[1]) +
ddeltau_dux[1] * du_dp[0][i]);
}
}
if(camera_at_identity)
{
if( dq_drcamera != NULL ) memset(dq_drcamera->xyz, 0, 6*sizeof(double));
if( dq_dtcamera != NULL ) memset(dq_dtcamera->xyz, 0, 6*sizeof(double));
if( dq_drframe != NULL ) propagate_extrinsics( dq_drframe, dp_drf );
if( dq_dtframe != NULL ) propagate_extrinsics_cam0( dq_dtframe );
}
else
{
if( dq_drcamera != NULL ) propagate_extrinsics( dq_drcamera, dp_drc );
if( dq_dtcamera != NULL ) propagate_extrinsics( dq_dtcamera, dp_dtc );
if( dq_drframe != NULL ) propagate_extrinsics( dq_drframe, dp_drf );
if( dq_dtframe != NULL ) propagate_extrinsics( dq_dtframe, dp_dtf );
}
}
typedef struct
{
double* pool;
uint16_t run_side_length;
uint16_t ivar_stridey;
} gradient_sparse_meta_t;
// Projects 3D point(s), and reports the projection, and all the gradients. This
// is the main internal callback in the optimizer. This operates in one of two modes:
//
// if(calibration_object_width_n == 0) then we're projecting ONE point. In world
// coords this point is at frame_rt->t. frame_rt->r is not referenced. q and the
// gradients reference 2 values (x,y in the imager)
//
// if(calibration_object_width_n > 0) then we're projecting a whole calibration
// object. The pose of this object is given in frame_rt. We project ALL
// calibration_object_width_n*calibration_object_height_n points. q and the
// gradients reference ALL of these points
static
void project( // out
mrcal_point2_t* restrict q,
// The intrinsics gradients. These are split among several arrays.
// High-parameter-count lens models can return their gradients
// sparsely. All the actual gradient values live in
// dq_dintrinsics_pool_double, a buffer supplied by the caller. If
// dq_dintrinsics_pool_double is not NULL, the rest of the
// variables are assumed non-NULL, and we compute all the
// intrinsics gradients. Conversely, if dq_dintrinsics_pool_double
// is NULL, no intrinsics gradients are computed
double* restrict dq_dintrinsics_pool_double,
int* restrict dq_dintrinsics_pool_int,
double** restrict dq_dfxy,
double** restrict dq_dintrinsics_nocore,
gradient_sparse_meta_t* gradient_sparse_meta,
mrcal_point3_t* restrict dq_drcamera,
mrcal_point3_t* restrict dq_dtcamera,
mrcal_point3_t* restrict dq_drframe,
mrcal_point3_t* restrict dq_dtframe,
mrcal_point2_t* restrict dq_dcalobject_warp,
// in
// everything; includes the core, if there is one
const double* restrict intrinsics,
const mrcal_pose_t* restrict camera_rt,
const mrcal_pose_t* restrict frame_rt,
const mrcal_point2_t* restrict calobject_warp,
bool camera_at_identity, // if true, camera_rt is unused
mrcal_lensmodel_t lensmodel,
const mrcal_projection_precomputed_t* precomputed,
double calibration_object_spacing,
int calibration_object_width_n,
int calibration_object_height_n)
{
assert(precomputed->ready);
// Parametric and non-parametric models do different things:
//
// parametric models:
// u = distort(p, distortions)
// q = uxy/uz * fxy + cxy
//
// extrinsic gradients:
// dqx/deee = d( ux/uz * fx + cx)/deee =
// = fx d(ux/uz)/deee =
// = fx/uz^2 ( uz dux/deee - duz/deee ux )
//
// nonparametric (splined) models
// u = stereographic(p)
// q = (u + deltau(u)) * f + c
//
// Extrinsics:
// dqx/deee = fx (dux/deee + ddeltaux/du du/deee)
// = fx ( dux/deee (1 + ddeltaux/dux) + ddeltaux/duy duy/deee)
// dqy/deee = fy ( duy/deee (1 + ddeltauy/duy) + ddeltauy/dux dux/deee)
//
// Intrinsics:
// dq/diii = f ddeltau/diii
//
// So the two kinds of models have completely different expressions for
// their gradients, and I implement them separately
const int Npoints =
calibration_object_width_n ?
calibration_object_width_n*calibration_object_height_n : 1;
const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel);
// I need to compose two transformations
//
// (object in reference frame) -> [frame transform] -> (object in the reference frame) ->
// -> [camera rt] -> (camera frame)
//
// Note that here the frame transform transforms TO the reference frame and
// the camera transform transforms FROM the reference frame. This is how my
// data is expected to be set up
//
// [Rc tc] [Rf tf] = [Rc*Rf Rc*tf + tc]
// [0 1 ] [0 1 ] [0 1 ]
//
// This transformation (and its gradients) is handled by mrcal_compose_rt()
// I refer to the camera*frame transform as the "joint" transform, or the
// letter j
geometric_gradients_t gg;
double _joint_rt[6];
double* joint_rt;
// The caller has an odd-looking array reference [-3]. This is intended, but
// the compiler throws a warning. I silence it here. gcc-10 produces a very
// strange-looking warning that was reported to the maintainers:
// https://gcc.gnu.org/bugzilla/show_bug.cgi?id=97261
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
mrcal_pose_t frame_rt_validr = {.t = frame_rt->t};
#pragma GCC diagnostic pop
if(calibration_object_width_n) frame_rt_validr.r = frame_rt->r;
if(!camera_at_identity)
{
// make sure I can pass mrcal_pose_t.r as an rt[] transformation
static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure");
static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure");
mrcal_compose_rt( _joint_rt,
gg._d_rj_rc, gg._d_rj_rf,
gg._d_tj_rc, gg._d_tj_tf,
camera_rt ->r.xyz,
frame_rt_validr.r.xyz);
joint_rt = _joint_rt;
}
else
{
// We're looking at the reference frame, so this camera transform is
// fixed at the identity. We don't need to compose anything, nor
// propagate gradients for the camera extrinsics, since those don't
// exist in the parameter vector
// Here the "joint" transform is just the "frame" transform
joint_rt = frame_rt_validr.r.xyz;
}
// Not using OpenCV distortions, the distortion and projection are not
// coupled
double Rj[3*3];
double d_Rj_rj[9*3];
mrcal_R_from_r(Rj, d_Rj_rj, joint_rt);
mrcal_point2_t* p_dq_dfxy = NULL;
double* p_dq_dintrinsics_nocore = NULL;
bool has_core = modelHasCore_fxfycxcy(lensmodel);
bool has_dense_intrinsics_grad = (lensmodel.type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC);
bool has_sparse_intrinsics_grad = (lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC);
if(dq_dintrinsics_pool_double != NULL)
{
// nothing by default
*dq_dfxy = NULL;
*dq_dintrinsics_nocore = NULL;
gradient_sparse_meta->pool = NULL;
int ivar_pool = 0;
if(has_core)
{
// Each point produces 2 measurements. Each one depends on ONE fxy
// element. So Npoints*2 of these
*dq_dfxy = &dq_dintrinsics_pool_double[ivar_pool];
p_dq_dfxy = (mrcal_point2_t*)*dq_dfxy;
ivar_pool += Npoints*2;
}
if(has_dense_intrinsics_grad)
{
*dq_dintrinsics_nocore = p_dq_dintrinsics_nocore = &dq_dintrinsics_pool_double[ivar_pool];
ivar_pool += Npoints*2 * (Nintrinsics-4);
}
if(has_sparse_intrinsics_grad)
{
if(lensmodel.type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC)
{
MSG("Unhandled lens model: %d (%s)",
lensmodel.type,
mrcal_lensmodel_name_unconfigured(lensmodel));
assert(0);
}
const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config =
&lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config;
*gradient_sparse_meta =
(gradient_sparse_meta_t)
{
.run_side_length = config->order+1,
.ivar_stridey = 2*config->Nx,
.pool = &dq_dintrinsics_pool_double[ivar_pool]
};
}
}
// These are produced by propagate_extrinsics() and consumed by
// project_point()
mrcal_point3_t _dp_drc[3];
mrcal_point3_t _dp_dtc[3];
mrcal_point3_t _dp_drf[3];
mrcal_point3_t _dp_dtf[3];
mrcal_point3_t* dp_drc;
mrcal_point3_t* dp_dtc;
mrcal_point3_t* dp_drf;
mrcal_point3_t* dp_dtf;
mrcal_point3_t propagate_extrinsics( const mrcal_point3_t* pt_ref,
const geometric_gradients_t* gg,
const double* Rj, const double* d_Rj_rj,
const double* _tj )
{
// Rj * pt + tj -> pt
mrcal_point3_t p;
mul_vec3_gen33t_vout(pt_ref->xyz, Rj, p.xyz);
add_vec(3, p.xyz, _tj);
void propagate_extrinsics_one(mrcal_point3_t* dp_dparam,
const double* drj_dparam,
const double* dtj_dparam,
const double* d_Rj_rj)
{
// dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0]
// dRj[row0]/drc = dRj[row0]/drj * drj_drc
for(int i=0; i<3; i++)
{
mul_vec3_gen33_vout( pt_ref->xyz, &d_Rj_rj[9*i], dp_dparam[i].xyz );
mul_vec3_gen33 ( dp_dparam[i].xyz, drj_dparam);
add_vec(3, dp_dparam[i].xyz, &dtj_dparam[3*i] );
}
}
void propagate_extrinsics_one_rzero(mrcal_point3_t* dp_dparam,
const double* dtj_dparam,
const double* d_Rj_rj)
{
// dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0]
// dRj[row0]/drc = dRj[row0]/drj * drj_drc
memcpy(dp_dparam->xyz, dtj_dparam, 9*sizeof(double));
}
void propagate_extrinsics_one_tzero(mrcal_point3_t* dp_dparam,
const double* drj_dparam,
const double* d_Rj_rj)
{
// dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0]
// dRj[row0]/drc = dRj[row0]/drj * drj_drc
for(int i=0; i<3; i++)
{
mul_vec3_gen33_vout( pt_ref->xyz, &d_Rj_rj[9*i], dp_dparam[i].xyz );
mul_vec3_gen33 ( dp_dparam[i].xyz, drj_dparam);
}
}
void propagate_extrinsics_one_rzero_tidentity(mrcal_point3_t* dp_dparam,
const double* d_Rj_rj)
{
dp_dparam[0] = (mrcal_point3_t){.x = 1.0};
dp_dparam[1] = (mrcal_point3_t){.y = 1.0};
dp_dparam[2] = (mrcal_point3_t){.z = 1.0};
}
void propagate_extrinsics_one_cam0(mrcal_point3_t* dp_rf,
const double* _d_Rf_rf)
{
// dRj[row0]/drj is 3x3 matrix at &_d_Rf_rf[0]
// dRj[row0]/drc = dRj[row0]/drj * drj_drc
for(int i=0; i<3; i++)
mul_vec3_gen33_vout( pt_ref->xyz, &_d_Rf_rf[9*i], dp_rf[i].xyz );
}
if(gg != NULL)
{
propagate_extrinsics_one( _dp_drc, gg->_d_rj_rc, gg->_d_tj_rc, d_Rj_rj);
propagate_extrinsics_one_rzero_tidentity(_dp_dtc, d_Rj_rj);
propagate_extrinsics_one_tzero( _dp_drf, gg->_d_rj_rf, d_Rj_rj);
propagate_extrinsics_one_rzero( _dp_dtf, gg->_d_tj_tf, d_Rj_rj);
dp_drc = _dp_drc;
dp_dtc = _dp_dtc;
dp_drf = _dp_drf;
dp_dtf = _dp_dtf;
}
else
{
// camera is at the reference. The "joint" coord system is the "frame"
// coord system
//
// p_cam = Rf p_ref + tf
//
// dp/drc = 0
// dp/dtc = 0
// dp/drf = reshape(dRf_drf p_ref)
// dp/dtf = I
propagate_extrinsics_one_cam0(_dp_drf, d_Rj_rj);
dp_drc = NULL;
dp_dtc = NULL;
dp_drf = _dp_drf;
dp_dtf = NULL; // this is I. The user of this MUST know to interpret
// it that way
}
return p;
}
void project_point( // outputs
mrcal_point2_t* q,
mrcal_point2_t* p_dq_dfxy,
double* p_dq_dintrinsics_nocore,
double* gradient_sparse_meta_pool,
int runlen,
mrcal_point3_t* restrict dq_drcamera,
mrcal_point3_t* restrict dq_dtcamera,
mrcal_point3_t* restrict dq_drframe,
mrcal_point3_t* restrict dq_dtframe,
mrcal_point2_t* restrict dq_dcalobject_warp,
// inputs
const mrcal_point3_t* p,
const double* restrict intrinsics,
mrcal_lensmodel_t lensmodel,
const mrcal_point2_t* dpt_ref2_dwarp,
// if NULL then the camera is at the reference
bool camera_at_identity,
const double* Rj)
{
if(lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC)
{
// only need 3+3 for quadratic splines
double grad_ABCDx_ABCDy[4+4];
int ivar0;
_project_point_splined( // outputs
q, p_dq_dfxy,
grad_ABCDx_ABCDy,
&ivar0,
dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe,
// inputs
p,
dp_drc, dp_dtc, dp_drf, dp_dtf,
intrinsics,
camera_at_identity,
lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order,
lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx,
lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny,
precomputed->LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed.segments_per_u);
if(dq_dintrinsics_pool_int != NULL)
{
*(dq_dintrinsics_pool_int++) = ivar0;
memcpy(gradient_sparse_meta_pool,
grad_ABCDx_ABCDy,
sizeof(double)*runlen*2);
}
}
else
{
_project_point_parametric( // outputs
q,p_dq_dfxy,
p_dq_dintrinsics_nocore,
dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe,
// inputs
p,
dp_drc, dp_dtc, dp_drf, dp_dtf,
intrinsics,
camera_at_identity,
lensmodel);
}
if( dq_dcalobject_warp != NULL && dpt_ref2_dwarp != NULL )
{
// p = proj(Rc Rf warp(x) + Rc tf + tc);
// dp/dw = dp/dRcRf(warp(x)) dR(warp(x))/dwarp(x) dwarp/dw =
// = dp/dtc RcRf dwarp/dw
// dp/dtc is dq_dtcamera
// R is rodrigues(rj)
// dwarp/dw = [0 0]
// [0 0]
// [a b]
// Let R = [r0 r1 r2]
// dp/dw = dp/dt [ar2 br2] = [a dp/dt r2 b dp/dt r2]
mrcal_point3_t* p_dq_dt;
if(!camera_at_identity) p_dq_dt = dq_dtcamera;
else p_dq_dt = dq_dtframe;
double d[] =
{ p_dq_dt[0].xyz[0] * Rj[0*3 + 2] +
p_dq_dt[0].xyz[1] * Rj[1*3 + 2] +
p_dq_dt[0].xyz[2] * Rj[2*3 + 2],
p_dq_dt[1].xyz[0] * Rj[0*3 + 2] +
p_dq_dt[1].xyz[1] * Rj[1*3 + 2] +
p_dq_dt[1].xyz[2] * Rj[2*3 + 2]};
dq_dcalobject_warp[0].x = d[0]*dpt_ref2_dwarp->x;
dq_dcalobject_warp[0].y = d[0]*dpt_ref2_dwarp->y;
dq_dcalobject_warp[1].x = d[1]*dpt_ref2_dwarp->x;
dq_dcalobject_warp[1].y = d[1]*dpt_ref2_dwarp->y;
}
}
int runlen = (lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) ?
(lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order + 1) :
0;
if( calibration_object_width_n == 0 )
{ // projecting discrete points
mrcal_point3_t p =
propagate_extrinsics( &(mrcal_point3_t){},
camera_at_identity ? NULL : &gg,
Rj, d_Rj_rj, &joint_rt[3]);
project_point( q,
p_dq_dfxy, p_dq_dintrinsics_nocore,
gradient_sparse_meta ? gradient_sparse_meta->pool : NULL,
runlen,
dq_drcamera, dq_dtcamera, dq_drframe, dq_dtframe, NULL,
&p,
intrinsics, lensmodel,
NULL,
camera_at_identity, Rj);
}
else
{ // projecting a chessboard
int i_pt = 0;
// The calibration object has a simple grid geometry
for(int y = 0; y<calibration_object_height_n; y++)
for(int x = 0; x<calibration_object_width_n; x++)
{
mrcal_point3_t pt_ref = {.x = (double)x * calibration_object_spacing,
.y = (double)y * calibration_object_spacing};
mrcal_point2_t dpt_ref2_dwarp = {};
if(calobject_warp != NULL)
{
// Add a board warp here. I have two parameters, and they describe
// additive flex along the x axis and along the y axis, in that
// order. In each direction the flex is a parabola, with the
// parameter k describing the max deflection at the center. If the
// ends are at +- 1 I have d = k*(1 - x^2). If the ends are at
// (0,N-1) the equivalent expression is: d = k*( 1 - 4*x^2/(N-1)^2 +
// 4*x/(N-1) - 1 ) = d = 4*k*(x/(N-1) - x^2/(N-1)^2) = d =
// 4.*k*x*r(1. - x*r)
double xr = (double)x / (double)(calibration_object_width_n -1);
double yr = (double)y / (double)(calibration_object_height_n-1);
double dx = 4. * xr * (1. - xr);
double dy = 4. * yr * (1. - yr);
pt_ref.z += calobject_warp->x * dx;
pt_ref.z += calobject_warp->y * dy;
dpt_ref2_dwarp.x = dx;
dpt_ref2_dwarp.y = dy;
}
mrcal_point3_t p =
propagate_extrinsics( &pt_ref,
camera_at_identity ? NULL : &gg,
Rj, d_Rj_rj, &joint_rt[3]);
mrcal_point3_t* dq_drcamera_here = dq_drcamera ? &dq_drcamera [2*i_pt] : NULL;
mrcal_point3_t* dq_dtcamera_here = dq_dtcamera ? &dq_dtcamera [2*i_pt] : NULL;
mrcal_point3_t* dq_drframe_here = dq_drframe ? &dq_drframe [2*i_pt] : NULL;
mrcal_point3_t* dq_dtframe_here = dq_dtframe ? &dq_dtframe [2*i_pt] : NULL;
mrcal_point2_t* dq_dcalobject_warp_here = dq_dcalobject_warp ? &dq_dcalobject_warp [2*i_pt] : NULL;
mrcal_point3_t dq_dtcamera_here_dummy[2];
mrcal_point3_t dq_dtframe_here_dummy [2];
if(dq_dcalobject_warp)
{
// I need all translation gradients to be available to
// compute the calobject_warp gradients (see the end of the
// project_point() function above). So I compute those even
// if the caller didn't ask for them
if(!dq_dtcamera_here) dq_dtcamera_here = dq_dtcamera_here_dummy;
if(!dq_dtframe_here) dq_dtframe_here = dq_dtframe_here_dummy;
}
project_point(&q[i_pt],
p_dq_dfxy ? &p_dq_dfxy[i_pt] : NULL,
p_dq_dintrinsics_nocore ? &p_dq_dintrinsics_nocore[2*(Nintrinsics-4)*i_pt] : NULL,
gradient_sparse_meta ? &gradient_sparse_meta->pool[i_pt*runlen*2] : NULL,
runlen,
dq_drcamera_here, dq_dtcamera_here, dq_drframe_here, dq_dtframe_here, dq_dcalobject_warp_here,
&p,
intrinsics, lensmodel,
&dpt_ref2_dwarp,
camera_at_identity, Rj);
i_pt++;
}
}
}
// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper
// only
bool _mrcal_project_internal_cahvore( // out
mrcal_point2_t* out,
// in
const mrcal_point3_t* p,
int N,
// core, distortions concatenated
const double* intrinsics)
{
// Apply a CAHVORE warp to an un-distorted point
// Given intrinsic parameters of a CAHVORE model and a set of
// camera-coordinate points, return the projected point(s)
// This comes from cmod_cahvore_3d_to_2d_general() in
// m-jplv/libcmod/cmod_cahvore.c
//
// The lack of documentation here comes directly from the lack of
// documentation in that function.
// I parametrize the optical axis such that
// - o(alpha=0, beta=0) = (0,0,1) i.e. the optical axis is at the center
// if both parameters are 0
// - The gradients are cartesian. I.e. do/dalpha and do/dbeta are both
// NOT 0 at (alpha=0,beta=0). This would happen at the poles (gimbal
// lock), and that would make my solver unhappy
// So o = { s_al*c_be, s_be, c_al*c_be }
const mrcal_intrinsics_core_t* core = (const mrcal_intrinsics_core_t*)intrinsics;
const double alpha = intrinsics[4 + 0];
const double beta = intrinsics[4 + 1];
const double r0 = intrinsics[4 + 2];
const double r1 = intrinsics[4 + 3];
const double r2 = intrinsics[4 + 4];
const double e0 = intrinsics[4 + 5];
const double e1 = intrinsics[4 + 6];
const double e2 = intrinsics[4 + 7];
const double linearity = intrinsics[4 + 8];
double sa,ca;
sincos(alpha, &sa, &ca);
double sb,cb;
sincos(beta, &sb, &cb);
const double o[] ={ cb * sa, sb, cb * ca };
for(int i_pt=0; i_pt<N; i_pt++)
{
///////////////// THIS IS MADE UP, AND PROBABLY WRONG
// I'm using jplv as the reference implementation for this, but that
// implementation can't work. In jplv project(p) and project(k*p) don't
// project to the same point, which they must for a valid projection
// function. Look at the definition of upsilon below. omega and l are
// proportional to the distance to the camera while the other terms are
// not. So if I'm looking at a point along the same observation ray, but
// 1000 times further out, omega and l will jump by a factor of 1000,
// while the other terms will not. I thus won't get the same projection
// result.
//
// I'm hypothesizing that they meant to normalize p, but never did it.
// So I'm doing that here. mrcal supports cahvore only for
// compatibility, so nobody's using this code. IF YOU ARE GOING TO USE
// THIS CODE, PLEASE CONFIRM THAT THIS CAHVORE PROJECTION IS CORRECT
double pnorm = sqrt(p[i_pt].x*p[i_pt].x +
p[i_pt].y*p[i_pt].y +
p[i_pt].z*p[i_pt].z );
double v[] =
{
p[i_pt].x / pnorm,
p[i_pt].y / pnorm,
p[i_pt].z / pnorm
};
// cos( angle between p and o ) = inner(p,o) / (norm(o) * norm(p)) =
// omega/norm(p)
double omega = v[0]*o[0] + v[1]*o[1] + v[2]*o[2];
// Basic Computations
// Calculate initial terms
double u[3];
for(int i=0; i<3; i++) u[i] = omega*o[i];
double ll[3];
for(int i=0; i<3; i++) ll[i] = v[i]-u[i];
double l = sqrt(ll[0]*ll[0] + ll[1]*ll[1] + ll[2]*ll[2]);
// Calculate theta using Newton's Method
double theta = atan2(l, omega);
int inewton;
for( inewton = 100; inewton; inewton--)
{
// Compute terms from the current value of theta
double sth,cth;
sincos(theta, &sth, &cth);
double theta2 = theta * theta;
double theta3 = theta * theta2;
double theta4 = theta * theta3;
double upsilon =
omega*cth + l*sth
- (1.0 - cth) * (e0 + e1*theta2 + e2*theta4)
- (theta - sth) * ( 2.0*e1*theta + 4.0*e2*theta3);
// Update theta
double dtheta =
(
omega*sth - l*cth
- (theta - sth) * (e0 + e1*theta2 + e2*theta4)
) / upsilon;
theta -= dtheta;
// Check exit criterion from last update
if(fabs(dtheta) < 1e-8)
break;
}
if(inewton == 0)
{
fprintf(stderr, "%s(): too many iterations\n", __func__);
return false;
}
// got a theta
// Check the value of theta
if(theta * fabs(linearity) > M_PI/2.)
{
fprintf(stderr, "%s(): theta out of bounds\n", __func__);
return false;
}
// If we aren't close enough to use the small-angle approximation ...
if (theta > 1e-8)
{
// ... do more math!
double linth = linearity * theta;
double chi;
if (linearity < -1e-15)
chi = sin(linth) / linearity;
else if (linearity > 1e-15)
chi = tan(linth) / linearity;
else
chi = theta;
double chi2 = chi * chi;
double chi3 = chi * chi2;
double chi4 = chi * chi3;
double zetap = l / chi;
double mu = r0 + r1*chi2 + r2*chi4;
double uu[3];
for(int i=0; i<3; i++) uu[i] = zetap*o[i];
double vv[3];
for(int i=0; i<3; i++) vv[i] = (1. + mu)*ll[i];
for(int i=0; i<3; i++)
u[i] = uu[i] + vv[i];
// now I apply a normal projection to the warped 3d point p
out[i_pt].x = core->focal_xy[0] * u[0]/u[2] + core->center_xy[0];
out[i_pt].y = core->focal_xy[1] * u[1]/u[2] + core->center_xy[1];
}
else
{
// now I apply a normal projection to the warped 3d point p
out[i_pt].x = core->focal_xy[0] * v[0]/v[2] + core->center_xy[0];
out[i_pt].y = core->focal_xy[1] * v[1]/v[2] + core->center_xy[1];
}
}
return true;
}
// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper
// only
bool _mrcal_project_internal( // out
mrcal_point2_t* q,
// Stored as a row-first array of shape (N,2,3). Each
// row lives in 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,
// in
const mrcal_point3_t* p,
int N,
mrcal_lensmodel_t lensmodel,
// core, distortions concatenated
const double* intrinsics,
int Nintrinsics,
const mrcal_projection_precomputed_t* precomputed)
{
if( dq_dintrinsics == NULL )
{
for(int i=0; i<N; i++)
{
mrcal_pose_t frame = {.r = {},
.t = p[i]};
// simple non-intrinsics-gradient path. dp_dp is handled entirely in
// project()
project( &q[i],
NULL, NULL, NULL, NULL, NULL,
NULL, NULL, NULL, dq_dp, NULL,
// in
intrinsics, NULL, &frame, NULL, true,
lensmodel, precomputed,
0.0, 0,0);
}
return true;
}
for(int i=0; i<N; i++)
{
mrcal_pose_t frame = {.r = {},
.t = p[i]};
// simple non-intrinsics-gradient path. dp_dp is handled entirely in
// project()
double dq_dintrinsics_pool_double[2*(1+Nintrinsics-4)];
int dq_dintrinsics_pool_int [1];
double* dq_dfxy = NULL;
double* dq_dintrinsics_nocore = NULL;
gradient_sparse_meta_t gradient_sparse_meta = {}; // init to pacify compiler warning
project( &q[i],
dq_dintrinsics_pool_double,
dq_dintrinsics_pool_int,
&dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta,
NULL, NULL, NULL, dq_dp, NULL,
// in
intrinsics, NULL, &frame, NULL, true,
lensmodel, precomputed,
0.0, 0,0);
int Ncore = 0;
if(dq_dfxy != NULL)
{
Ncore = 4;
// fxy. off-diagonal elements are 0
dq_dintrinsics[0*Nintrinsics + 0] = dq_dfxy[0];
dq_dintrinsics[0*Nintrinsics + 1] = 0.0;
dq_dintrinsics[1*Nintrinsics + 0] = 0.0;
dq_dintrinsics[1*Nintrinsics + 1] = dq_dfxy[1];
// cxy. Identity
dq_dintrinsics[0*Nintrinsics + 2] = 1.0;
dq_dintrinsics[0*Nintrinsics + 3] = 0.0;
dq_dintrinsics[1*Nintrinsics + 2] = 0.0;
dq_dintrinsics[1*Nintrinsics + 3] = 1.0;
}
if( dq_dintrinsics_nocore != NULL )
{
for(int i_xy=0; i_xy<2; i_xy++)
memcpy(&dq_dintrinsics[i_xy*Nintrinsics + Ncore],
&dq_dintrinsics_nocore[i_xy*(Nintrinsics-Ncore)],
(Nintrinsics-Ncore)*sizeof(double));
}
if(gradient_sparse_meta.pool != NULL)
{
// u = stereographic(p)
// q = (u + deltau(u)) * f + c
//
// Intrinsics:
// dq/diii = f ddeltau/diii
//
// ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3])
const int ivar0 = dq_dintrinsics_pool_int[0];
const int len = gradient_sparse_meta.run_side_length;
const double* ABCDx = &gradient_sparse_meta.pool[0];
const double* ABCDy = &gradient_sparse_meta.pool[len];
const int ivar_stridey = gradient_sparse_meta.ivar_stridey;
const double* fxy = &intrinsics[0];
for(int i_xy=0; i_xy<2; i_xy++)
for(int iy=0; iy<len; iy++)
for(int ix=0; ix<len; ix++)
{
int ivar = ivar0 + ivar_stridey*iy + ix*2 + i_xy;
dq_dintrinsics[ivar + i_xy*Nintrinsics] =
ABCDx[ix]*ABCDy[iy]*fxy[i_xy];
}
}
// advance
dq_dintrinsics = &dq_dintrinsics[2*Nintrinsics];
if(dq_dp != NULL)
dq_dp = &dq_dp[2];
}
return true;
}
// External interface to the internal project() function. The internal function
// is more general (supports geometric transformations prior to projection, and
// supports chessboards). dq_dintrinsics and/or dq_dp are allowed to be NULL if
// we're not interested in gradients.
//
// This function supports CAHVORE distortions if we don't ask for gradients
//
// Projecting out-of-bounds points (beyond the field of view) returns undefined
// values. Generally things remain continuous even as we move off the imager
// domain. Pinhole-like projections will work normally if projecting a point
// behind the camera. Splined projections clamp to the nearest spline segment:
// the projection will fly off to infinity quickly since we're extrapolating a
// polynomial, but the function will remain continuous.
bool mrcal_project( // out
mrcal_point2_t* q,
// Stored as a row-first array of shape (N,2,3). Each row
// lives in a mrcal_point3_t. May be NULL
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. May be NULL
double* dq_dintrinsics,
// in
const mrcal_point3_t* p,
int N,
mrcal_lensmodel_t lensmodel,
// core, distortions concatenated
const double* intrinsics)
{
// The outer logic (outside the loop-over-N-points) is duplicated in
// mrcal_project() and in the python wrapper definition in _project() and
// _project_withgrad() in mrcal-genpywrap.py. Please keep them in sync
// project() doesn't handle cahvore, so I special-case it here
if( lensmodel.type == MRCAL_LENSMODEL_CAHVORE )
{
if(dq_dintrinsics != NULL || dq_dp != NULL)
{
fprintf(stderr, "mrcal_project(MRCAL_LENSMODEL_CAHVORE) is not yet implemented if we're asking for gradients\n");
return false;
}
return _mrcal_project_internal_cahvore(q, p, N, intrinsics);
}
int Nintrinsics = mrcal_lensmodel_num_params(lensmodel);
// Special-case for opencv/pinhole and projection-only. cvProjectPoints2 and
// project() have a lot of overhead apparently, and calling either in a loop
// is very slow. I can call it once, and use its fast internal loop,
// however. This special case does the same thing, but much faster.
if(dq_dintrinsics == NULL && dq_dp == NULL &&
(MRCAL_LENSMODEL_IS_OPENCV(lensmodel.type) ||
lensmodel.type == MRCAL_LENSMODEL_PINHOLE))
{
_mrcal_project_internal_opencv( q, NULL,NULL,
p, N, intrinsics, Nintrinsics);
return true;
}
// Some models have sparse gradients, but I'm returning a dense array here.
// So I init everything at 0
if(dq_dintrinsics != NULL)
memset(dq_dintrinsics, 0, N*2*Nintrinsics*sizeof(double));
mrcal_projection_precomputed_t precomputed;
_mrcal_precompute_lensmodel_data(&precomputed, lensmodel);
return
_mrcal_project_internal(q, dq_dp, dq_dintrinsics,
p, N, lensmodel, intrinsics,
Nintrinsics, &precomputed);
}
// Maps a set of distorted 2D imager points q to a 3D vector in camera
// coordinates that produced these pixel observations. The 3D vector is defined
// up-to-length. The returned vectors v are not normalized, and may have any
// length.
//
// This is the "reverse" direction, so an iterative nonlinear optimization is
// performed internally to compute this result. This is much slower than
// mrcal_project. For OpenCV distortions specifically, OpenCV has
// cvUndistortPoints() (and cv2.undistortPoints()), but these are inaccurate:
// https://github.com/opencv/opencv/issues/8811
//
// This function does NOT support CAHVORE
bool mrcal_unproject( // out
mrcal_point3_t* out,
// in
const mrcal_point2_t* q,
int N,
mrcal_lensmodel_t lensmodel,
// core, distortions concatenated
const double* intrinsics)
{
if( lensmodel.type == MRCAL_LENSMODEL_CAHVORE )
{
fprintf(stderr, "mrcal_unproject(MRCAL_LENSMODEL_CAHVORE) not yet implemented. No gradients available\n");
return false;
}
// easy special-cases
if( lensmodel.type == MRCAL_LENSMODEL_PINHOLE )
{
double fx = intrinsics[0];
double fy = intrinsics[1];
double cx = intrinsics[2];
double cy = intrinsics[3];
for(int i=0; i<N; i++)
{
out->x = (q[i].x - cx) / fx;
out->y = (q[i].y - cy) / fy;
out->z = 1.0;
// advance
out++;
}
return true;
}
if( lensmodel.type == MRCAL_LENSMODEL_STEREOGRAPHIC )
{
double fx = intrinsics[0];
double fy = intrinsics[1];
double cx = intrinsics[2];
double cy = intrinsics[3];
mrcal_unproject_stereographic(out, NULL, q, N, fx,fy,cx,cy);
return true;
}
mrcal_projection_precomputed_t precomputed;
_mrcal_precompute_lensmodel_data(&precomputed, lensmodel);
return _mrcal_unproject_internal(out, q, N, lensmodel, intrinsics, &precomputed);
}
// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper
// only
bool _mrcal_unproject_internal( // out
mrcal_point3_t* out,
// in
const mrcal_point2_t* q,
int N,
mrcal_lensmodel_t lensmodel,
// core, distortions concatenated
const double* intrinsics,
const mrcal_projection_precomputed_t* precomputed)
{
double fx = intrinsics[0];
double fy = intrinsics[1];
double cx = intrinsics[2];
double cy = intrinsics[3];
// I optimize in the space of the stereographic projection. This is a 2D
// space with a direct mapping to/from observation vectors with a single
// singularity directly behind the camera. The allows me to run an
// unconstrained optimization here
for(int i=0; i<N; i++)
{
void cb(const double* u,
double* x,
double* J,
void* cookie __attribute__((unused)))
{
// u is the constant-fxy-cxy 2D stereographic
// projection of the hypothesis v. I unproject it stereographically,
// and project it using the actual model
mrcal_point2_t dv_du[3];
mrcal_pose_t frame = {};
mrcal_unproject_stereographic( &frame.t, dv_du,
(mrcal_point2_t*)u, 1,
fx,fy,cx,cy );
mrcal_point3_t dq_dtframe[2];
mrcal_point2_t q_hypothesis;
project( &q_hypothesis,
NULL,NULL,NULL,NULL,NULL,
NULL, NULL, NULL, dq_dtframe,
NULL,
// in
intrinsics,
NULL,
&frame,
NULL,
true,
lensmodel, precomputed,
0.0, 0,0);
x[0] = q_hypothesis.x - q[i].x;
x[1] = q_hypothesis.y - q[i].y;
J[0*2 + 0] =
dq_dtframe[0].x*dv_du[0].x +
dq_dtframe[0].y*dv_du[1].x +
dq_dtframe[0].z*dv_du[2].x;
J[0*2 + 1] =
dq_dtframe[0].x*dv_du[0].y +
dq_dtframe[0].y*dv_du[1].y +
dq_dtframe[0].z*dv_du[2].y;
J[1*2 + 0] =
dq_dtframe[1].x*dv_du[0].x +
dq_dtframe[1].y*dv_du[1].x +
dq_dtframe[1].z*dv_du[2].x;
J[1*2 + 1] =
dq_dtframe[1].x*dv_du[0].y +
dq_dtframe[1].y*dv_du[1].y +
dq_dtframe[1].z*dv_du[2].y;
}
#warning "This should go away. For some reason it makes unproject() converge better, and it makes the tests pass. But it's not even right!"
#if 0
out->xyz[0] = (q[i].x-cx)/fx;
out->xyz[1] = (q[i].y-cy)/fy;
#else
// Seed from a perfect stereographic projection, pushed towards the
// center a bit. Normally I'd set out[] to q[i], but for some models
// (OPENCV8 for instance) this pushes us into a place where stuff
// doesn't converge anymore. This produces a more stable solution, and
// my tests pass
out->xyz[0] = (q[i].x-cx)*0.7 + cx;
out->xyz[1] = (q[i].y-cy)*0.7 + cy;
// something like this makes more sense, but it doesn't work! The tests still fail
// out->xyz[0] = (q[i].x-cx)/fx * 0.7;
// out->xyz[1] = (q[i].y-cy)/fy * 0.7;
#endif
dogleg_parameters2_t dogleg_parameters;
dogleg_getDefaultParameters(&dogleg_parameters);
dogleg_parameters.dogleg_debug = 0;
double norm2x =
dogleg_optimize_dense2(out->xyz, 2, 2, cb, NULL,
&dogleg_parameters,
NULL);
//This needs to be precise; if it isn't, I barf. Shouldn't happen
//very often
static bool already_complained = false;
if(norm2x/2.0 > 1e-4)
{
if(!already_complained)
{
// MSG("WARNING: I wasn't able to precisely compute some points. norm2x=%f. Returning nan for those. Will complain just once",
// norm2x);
already_complained = true;
}
double nan = strtod("NAN", NULL);
out->xyz[0] = nan;
out->xyz[1] = nan;
}
else
{
// out[0,1] is the stereographic representation of the observation
// vector using idealized fx,fy,cx,cy. This is already the right
// thing if we're reporting in 2d. Otherwise I need to unproject
// This is the normal no-error path
mrcal_unproject_stereographic((mrcal_point3_t*)out, NULL,
(mrcal_point2_t*)out, 1,
fx,fy,cx,cy);
if(!model_supports_projection_behind_camera(lensmodel) && out->xyz[2] < 0.0)
{
out->xyz[0] *= -1.0;
out->xyz[1] *= -1.0;
out->xyz[2] *= -1.0;
}
}
// Advance to the next point. Error or not
out++;
}
return true;
}
// The following functions define/use the layout of the state vector. In general
// I do:
//
// intrinsics_cam0
// intrinsics_cam1
// intrinsics_cam2
// ...
// extrinsics_cam1
// extrinsics_cam2
// extrinsics_cam3
// ...
// frame0
// frame1
// frame2
// ....
// calobject_warp0
// calobject_warp1
// From real values to unit-scale values. Optimizer sees unit-scale values
static int pack_solver_state_intrinsics( // out
double* p, // subset based on problem_details
// in
const double* intrinsics, // ALL variables. Not a subset
const mrcal_lensmodel_t lensmodel,
mrcal_problem_details_t problem_details,
int Ncameras_intrinsics )
{
int i_state = 0;
const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel);
const int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0;
const int Ndistortions = Nintrinsics - Ncore;
for(int icam_intrinsics=0; icam_intrinsics < Ncameras_intrinsics; icam_intrinsics++)
{
if( problem_details.do_optimize_intrinsics_core && Ncore )
{
const mrcal_intrinsics_core_t* intrinsics_core = (const mrcal_intrinsics_core_t*)intrinsics;
p[i_state++] = intrinsics_core->focal_xy [0] / SCALE_INTRINSICS_FOCAL_LENGTH;
p[i_state++] = intrinsics_core->focal_xy [1] / SCALE_INTRINSICS_FOCAL_LENGTH;
p[i_state++] = intrinsics_core->center_xy[0] / SCALE_INTRINSICS_CENTER_PIXEL;
p[i_state++] = intrinsics_core->center_xy[1] / SCALE_INTRINSICS_CENTER_PIXEL;
}
if( problem_details.do_optimize_intrinsics_distortions )
for(int i = 0; i<Ndistortions; i++)
p[i_state++] = intrinsics[Ncore + i] / SCALE_DISTORTION;
intrinsics = &intrinsics[Nintrinsics];
}
return i_state;
}
// It is ugly to have this as separate from pack_solver_state_intrinsics(), but
// I am tired. THIS function uses only intrinsic arrays that respect the skipped
// optimization variables in problem_details. And this function works inline
static int pack_solver_state_intrinsics_subset_to_subset( // out,in
double* p, // subset based on problem_details
// in
const mrcal_lensmodel_t lensmodel,
mrcal_problem_details_t problem_details,
int Ncameras_intrinsics )
{
if( !problem_details.do_optimize_intrinsics_core &&
!problem_details.do_optimize_intrinsics_distortions )
return 0;
int i_state = 0;
const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel);
const int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0;
const int Ndistortions = Nintrinsics - Ncore;
for(int icam_intrinsics=0; icam_intrinsics < Ncameras_intrinsics; icam_intrinsics++)
{
if( problem_details.do_optimize_intrinsics_core && Ncore )
{
p[i_state++] /= SCALE_INTRINSICS_FOCAL_LENGTH;
p[i_state++] /= SCALE_INTRINSICS_FOCAL_LENGTH;
p[i_state++] /= SCALE_INTRINSICS_CENTER_PIXEL;
p[i_state++] /= SCALE_INTRINSICS_CENTER_PIXEL;
}
if( problem_details.do_optimize_intrinsics_distortions )
for(int i = 0; i<Ndistortions; i++)
p[i_state++] /= SCALE_DISTORTION;
}
return i_state;
}
static void pack_solver_state( // out
double* p,
// in
const mrcal_lensmodel_t lensmodel,
const double* intrinsics, // Ncameras_intrinsics of these
const mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these
const mrcal_pose_t* frames_toref, // Nframes of these
const mrcal_point3_t* points, // Npoints of these
const mrcal_point2_t* calobject_warp, // 1 of these
mrcal_problem_details_t problem_details,
int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
int Npoints_variable,
int Nstate_ref)
{
int i_state = 0;
i_state += pack_solver_state_intrinsics( p, intrinsics,
lensmodel, problem_details,
Ncameras_intrinsics );
if( problem_details.do_optimize_extrinsics )
for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++)
{
p[i_state++] = extrinsics_fromref[icam_extrinsics].r.xyz[0] / SCALE_ROTATION_CAMERA;
p[i_state++] = extrinsics_fromref[icam_extrinsics].r.xyz[1] / SCALE_ROTATION_CAMERA;
p[i_state++] = extrinsics_fromref[icam_extrinsics].r.xyz[2] / SCALE_ROTATION_CAMERA;
p[i_state++] = extrinsics_fromref[icam_extrinsics].t.xyz[0] / SCALE_TRANSLATION_CAMERA;
p[i_state++] = extrinsics_fromref[icam_extrinsics].t.xyz[1] / SCALE_TRANSLATION_CAMERA;
p[i_state++] = extrinsics_fromref[icam_extrinsics].t.xyz[2] / SCALE_TRANSLATION_CAMERA;
}
if( problem_details.do_optimize_frames )
{
for(int iframe = 0; iframe < Nframes; iframe++)
{
p[i_state++] = frames_toref[iframe].r.xyz[0] / SCALE_ROTATION_FRAME;
p[i_state++] = frames_toref[iframe].r.xyz[1] / SCALE_ROTATION_FRAME;
p[i_state++] = frames_toref[iframe].r.xyz[2] / SCALE_ROTATION_FRAME;
p[i_state++] = frames_toref[iframe].t.xyz[0] / SCALE_TRANSLATION_FRAME;
p[i_state++] = frames_toref[iframe].t.xyz[1] / SCALE_TRANSLATION_FRAME;
p[i_state++] = frames_toref[iframe].t.xyz[2] / SCALE_TRANSLATION_FRAME;
}
for(int i_point = 0; i_point < Npoints_variable; i_point++)
{
p[i_state++] = points[i_point].xyz[0] / SCALE_POSITION_POINT;
p[i_state++] = points[i_point].xyz[1] / SCALE_POSITION_POINT;
p[i_state++] = points[i_point].xyz[2] / SCALE_POSITION_POINT;
}
}
if( problem_details.do_optimize_calobject_warp )
{
p[i_state++] = calobject_warp->x / SCALE_CALOBJECT_WARP;
p[i_state++] = calobject_warp->y / SCALE_CALOBJECT_WARP;
}
assert(i_state == Nstate_ref);
}
// Same as above, but packs/unpacks a vector instead of structures
void mrcal_pack_solver_state_vector( // out, in
double* p, // FULL state on input, unitless
// state on output
// in
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed,
mrcal_problem_details_t problem_details,
const mrcal_lensmodel_t lensmodel)
{
int Npoints_variable = Npoints - Npoints_fixed;
int i_state = 0;
i_state += pack_solver_state_intrinsics_subset_to_subset( p,
lensmodel, problem_details,
Ncameras_intrinsics );
static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure");
static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure");
if( problem_details.do_optimize_extrinsics )
for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++)
{
mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&p[i_state]);
p[i_state++] = extrinsics_fromref->r.xyz[0] / SCALE_ROTATION_CAMERA;
p[i_state++] = extrinsics_fromref->r.xyz[1] / SCALE_ROTATION_CAMERA;
p[i_state++] = extrinsics_fromref->r.xyz[2] / SCALE_ROTATION_CAMERA;
p[i_state++] = extrinsics_fromref->t.xyz[0] / SCALE_TRANSLATION_CAMERA;
p[i_state++] = extrinsics_fromref->t.xyz[1] / SCALE_TRANSLATION_CAMERA;
p[i_state++] = extrinsics_fromref->t.xyz[2] / SCALE_TRANSLATION_CAMERA;
}
if( problem_details.do_optimize_frames )
{
for(int iframe = 0; iframe < Nframes; iframe++)
{
mrcal_pose_t* frames_toref = (mrcal_pose_t*)(&p[i_state]);
p[i_state++] = frames_toref->r.xyz[0] / SCALE_ROTATION_FRAME;
p[i_state++] = frames_toref->r.xyz[1] / SCALE_ROTATION_FRAME;
p[i_state++] = frames_toref->r.xyz[2] / SCALE_ROTATION_FRAME;
p[i_state++] = frames_toref->t.xyz[0] / SCALE_TRANSLATION_FRAME;
p[i_state++] = frames_toref->t.xyz[1] / SCALE_TRANSLATION_FRAME;
p[i_state++] = frames_toref->t.xyz[2] / SCALE_TRANSLATION_FRAME;
}
for(int i_point = 0; i_point < Npoints_variable; i_point++)
{
mrcal_point3_t* points = (mrcal_point3_t*)(&p[i_state]);
p[i_state++] = points->xyz[0] / SCALE_POSITION_POINT;
p[i_state++] = points->xyz[1] / SCALE_POSITION_POINT;
p[i_state++] = points->xyz[2] / SCALE_POSITION_POINT;
}
}
if( problem_details.do_optimize_calobject_warp )
{
mrcal_point2_t* calobject_warp = (mrcal_point2_t*)(&p[i_state]);
p[i_state++] = calobject_warp->x / SCALE_CALOBJECT_WARP;
p[i_state++] = calobject_warp->y / SCALE_CALOBJECT_WARP;
}
}
static int unpack_solver_state_intrinsics( // out
// Ncameras_intrinsics of these
double* intrinsics, // ALL variables. Not a subset.
// I don't touch the elemnents I'm
// not optimizing
// in
const double* p, // subset based on problem_details
const mrcal_lensmodel_t lensmodel,
mrcal_problem_details_t problem_details,
int intrinsics_stride,
int Ncameras_intrinsics )
{
if( !problem_details.do_optimize_intrinsics_core &&
!problem_details.do_optimize_intrinsics_distortions )
return 0;
const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel);
const int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0;
int i_state = 0;
for(int icam_intrinsics=0; icam_intrinsics < Ncameras_intrinsics; icam_intrinsics++)
{
if( problem_details.do_optimize_intrinsics_core && Ncore )
{
intrinsics[icam_intrinsics*intrinsics_stride + 0] = p[i_state++] * SCALE_INTRINSICS_FOCAL_LENGTH;
intrinsics[icam_intrinsics*intrinsics_stride + 1] = p[i_state++] * SCALE_INTRINSICS_FOCAL_LENGTH;
intrinsics[icam_intrinsics*intrinsics_stride + 2] = p[i_state++] * SCALE_INTRINSICS_CENTER_PIXEL;
intrinsics[icam_intrinsics*intrinsics_stride + 3] = p[i_state++] * SCALE_INTRINSICS_CENTER_PIXEL;
}
if( problem_details.do_optimize_intrinsics_distortions )
{
for(int i = 0; i<Nintrinsics-Ncore; i++)
intrinsics[icam_intrinsics*intrinsics_stride + Ncore + i] = p[i_state++] * SCALE_DISTORTION;
}
}
return i_state;
}
// It is ugly to have this as separate from unpack_solver_state_intrinsics(),
// but I am tired. THIS function uses only intrinsic arrays that respect the
// skipped optimization variables in problem_details. And this function works
// inline
static int unpack_solver_state_intrinsics_subset_to_subset( // in,out
double* p, // subset based on problem_details
// in
const mrcal_lensmodel_t lensmodel,
mrcal_problem_details_t problem_details,
int Ncameras_intrinsics )
{
if( !problem_details.do_optimize_intrinsics_core &&
!problem_details.do_optimize_intrinsics_distortions )
return 0;
int i_state = 0;
const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel);
const int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0;
const int Ndistortions = Nintrinsics - Ncore;
for(int icam_intrinsics=0; icam_intrinsics < Ncameras_intrinsics; icam_intrinsics++)
{
if( problem_details.do_optimize_intrinsics_core && Ncore )
{
p[i_state++] *= SCALE_INTRINSICS_FOCAL_LENGTH;
p[i_state++] *= SCALE_INTRINSICS_FOCAL_LENGTH;
p[i_state++] *= SCALE_INTRINSICS_CENTER_PIXEL;
p[i_state++] *= SCALE_INTRINSICS_CENTER_PIXEL;
}
if( problem_details.do_optimize_intrinsics_distortions )
for(int i = 0; i<Ndistortions; i++)
p[i_state++] *= SCALE_DISTORTION;
}
return i_state;
}
static int unpack_solver_state_extrinsics_one(// out
mrcal_pose_t* extrinsic,
// in
const double* p)
{
int i_state = 0;
extrinsic->r.xyz[0] = p[i_state++] * SCALE_ROTATION_CAMERA;
extrinsic->r.xyz[1] = p[i_state++] * SCALE_ROTATION_CAMERA;
extrinsic->r.xyz[2] = p[i_state++] * SCALE_ROTATION_CAMERA;
extrinsic->t.xyz[0] = p[i_state++] * SCALE_TRANSLATION_CAMERA;
extrinsic->t.xyz[1] = p[i_state++] * SCALE_TRANSLATION_CAMERA;
extrinsic->t.xyz[2] = p[i_state++] * SCALE_TRANSLATION_CAMERA;
return i_state;
}
static int unpack_solver_state_framert_one(// out
mrcal_pose_t* frame,
// in
const double* p)
{
int i_state = 0;
frame->r.xyz[0] = p[i_state++] * SCALE_ROTATION_FRAME;
frame->r.xyz[1] = p[i_state++] * SCALE_ROTATION_FRAME;
frame->r.xyz[2] = p[i_state++] * SCALE_ROTATION_FRAME;
frame->t.xyz[0] = p[i_state++] * SCALE_TRANSLATION_FRAME;
frame->t.xyz[1] = p[i_state++] * SCALE_TRANSLATION_FRAME;
frame->t.xyz[2] = p[i_state++] * SCALE_TRANSLATION_FRAME;
return i_state;
}
static int unpack_solver_state_point_one(// out
mrcal_point3_t* point,
// in
const double* p)
{
int i_state = 0;
point->xyz[0] = p[i_state++] * SCALE_POSITION_POINT;
point->xyz[1] = p[i_state++] * SCALE_POSITION_POINT;
point->xyz[2] = p[i_state++] * SCALE_POSITION_POINT;
return i_state;
}
static int unpack_solver_state_calobject_warp(// out
mrcal_point2_t* calobject_warp,
// in
const double* p)
{
int i_state = 0;
calobject_warp->xy[0] = p[i_state++] * SCALE_CALOBJECT_WARP;
calobject_warp->xy[1] = p[i_state++] * SCALE_CALOBJECT_WARP;
return i_state;
}
// From unit-scale values to real values. Optimizer sees unit-scale values
static void unpack_solver_state( // out
// ALL intrinsics; whether we're optimizing
// them or not. Don't touch the parts of this
// array we're not optimizing
double* intrinsics_all, // Ncameras_intrinsics of these
mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these
mrcal_pose_t* frames_toref, // Nframes of these
mrcal_point3_t* points, // Npoints of these
mrcal_point2_t* calobject_warp, // 1 of these
// in
const double* p,
const mrcal_lensmodel_t lensmodel,
mrcal_problem_details_t problem_details,
int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, int Npoints_variable,
int Nstate_ref)
{
int i_state = unpack_solver_state_intrinsics(intrinsics_all,
p, lensmodel, problem_details,
mrcal_lensmodel_num_params(lensmodel),
Ncameras_intrinsics);
if( problem_details.do_optimize_extrinsics )
for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++)
i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &p[i_state] );
if( problem_details.do_optimize_frames )
{
for(int iframe = 0; iframe < Nframes; iframe++)
i_state += unpack_solver_state_framert_one( &frames_toref[iframe], &p[i_state] );
for(int i_point = 0; i_point < Npoints_variable; i_point++)
i_state += unpack_solver_state_point_one( &points[i_point], &p[i_state] );
}
if( problem_details.do_optimize_calobject_warp )
i_state += unpack_solver_state_calobject_warp(calobject_warp, &p[i_state]);
assert(i_state == Nstate_ref);
}
// Same as above, but packs/unpacks a vector instead of structures
void mrcal_unpack_solver_state_vector( // out, in
double* p, // unitless state on input,
// scaled, meaningful state on
// output
// in
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed,
mrcal_problem_details_t problem_details,
const mrcal_lensmodel_t lensmodel)
{
int Npoints_variable = Npoints - Npoints_fixed;
int i_state =
unpack_solver_state_intrinsics_subset_to_subset(p,
lensmodel, problem_details,
Ncameras_intrinsics);
if( problem_details.do_optimize_extrinsics )
{
static_assert( offsetof(mrcal_pose_t, r) == 0,
"mrcal_pose_t has expected structure");
static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double),
"mrcal_pose_t has expected structure");
mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&p[i_state]);
for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++)
i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &p[i_state] );
}
if( problem_details.do_optimize_frames )
{
mrcal_pose_t* frames_toref = (mrcal_pose_t*)(&p[i_state]);
for(int iframe = 0; iframe < Nframes; iframe++)
i_state += unpack_solver_state_framert_one( &frames_toref[iframe], &p[i_state] );
mrcal_point3_t* points = (mrcal_point3_t*)(&p[i_state]);
for(int i_point = 0; i_point < Npoints_variable; i_point++)
i_state += unpack_solver_state_point_one( &points[i_point], &p[i_state] );
}
if( problem_details.do_optimize_calobject_warp )
{
mrcal_point2_t* calobject_warp = (mrcal_point2_t*)(&p[i_state]);
i_state += unpack_solver_state_calobject_warp(calobject_warp, &p[i_state]);
}
}
int mrcal_state_index_intrinsics(int icam_intrinsics,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return icam_intrinsics * mrcal_num_intrinsics_optimization_params(problem_details, lensmodel);
}
int mrcal_num_states_intrinsics(int Ncameras_intrinsics,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return
Ncameras_intrinsics *
mrcal_num_intrinsics_optimization_params(problem_details, lensmodel);
}
int mrcal_state_index_extrinsics(int icam_extrinsics,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return
mrcal_num_states_intrinsics(Ncameras_intrinsics,
problem_details,
lensmodel) +
(problem_details.do_optimize_extrinsics ? (icam_extrinsics*6) : 0);
}
int mrcal_num_states_extrinsics(int Ncameras_extrinsics,
mrcal_problem_details_t problem_details)
{
return problem_details.do_optimize_extrinsics ? (6*Ncameras_extrinsics) : 0;
}
int mrcal_state_index_frames(int iframe,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return
mrcal_num_states_intrinsics(Ncameras_intrinsics,
problem_details,
lensmodel) +
mrcal_num_states_extrinsics(Ncameras_extrinsics,
problem_details) +
(problem_details.do_optimize_frames ? (iframe*6) : 0);
}
int mrcal_num_states_frames(int Nframes,
mrcal_problem_details_t problem_details)
{
return problem_details.do_optimize_frames ? (6*Nframes) : 0;
}
int mrcal_state_index_points(int i_point,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return
mrcal_num_states_intrinsics(Ncameras_intrinsics,
problem_details,
lensmodel) +
mrcal_num_states_extrinsics(Ncameras_extrinsics,
problem_details) +
mrcal_num_states_frames (Nframes,
problem_details) +
(problem_details.do_optimize_frames ? (i_point*3) : 0);
}
int mrcal_num_states_points(int Npoints, int Npoints_fixed,
mrcal_problem_details_t problem_details)
{
int Npoints_variable = Npoints - Npoints_fixed;
return problem_details.do_optimize_frames ? (Npoints_variable*3) : 0;
}
int mrcal_state_index_calobject_warp(int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_details_t problem_details,
mrcal_lensmodel_t lensmodel)
{
return
mrcal_num_states_intrinsics(Ncameras_intrinsics,
problem_details,
lensmodel) +
mrcal_num_states_extrinsics(Ncameras_extrinsics,
problem_details) +
mrcal_num_states_frames (Nframes,
problem_details) +
mrcal_num_states_points (Npoints, Npoints_fixed,
problem_details);
}
int mrcal_num_states_calobject_warp(mrcal_problem_details_t problem_details,
int Nobservations_board)
{
if(problem_details.do_optimize_calobject_warp && Nobservations_board>0)
return 2;
return 0;
}
// Reports the icam_extrinsics corresponding to a given icam_intrinsics. On
// success, the result is written to *icam_extrinsics, and we return true. If
// the given camera is at the reference coordinate system, it has no extrinsics,
// and we report -1. This query only makes sense for a calibration problem:
// we're observing a moving object with stationary cameras. If we have moving
// cameras, there won't be a single icam_extrinsics for a given icam_intrinsics,
// and we report an error by returning false
bool mrcal_corresponding_icam_extrinsics(// out
int* icam_extrinsics,
// in
int icam_intrinsics,
int Ncameras_intrinsics,
int Ncameras_extrinsics,
int Nobservations_board,
const mrcal_observation_board_t* observations_board,
int Nobservations_point,
const mrcal_observation_point_t* observations_point)
{
if( !(Ncameras_intrinsics == Ncameras_extrinsics ||
Ncameras_intrinsics == Ncameras_extrinsics+1 ) )
{
MSG("Cannot compute icam_extrinsics. I don't have a pure calibration problem");
return false;
}
int icam_map_to_extrinsics[Ncameras_intrinsics];
int icam_map_to_intrinsics[Ncameras_extrinsics+1];
for(int i=0; i<Ncameras_intrinsics; i++) icam_map_to_extrinsics[i] = -100;
for(int i=0; i<Ncameras_extrinsics+1; i++) icam_map_to_intrinsics[i] = -100;
bool check( const mrcal_camera_index_t* icam, int i, const char* what)
{
int icam_intrinsics = icam->intrinsics;
int icam_extrinsics = icam->extrinsics;
if(icam_extrinsics < 0) icam_extrinsics = -1;
if(icam_map_to_intrinsics[icam_extrinsics+1] == -100)
icam_map_to_intrinsics[icam_extrinsics+1] = icam_intrinsics;
else if(icam_map_to_intrinsics[icam_extrinsics+1] != icam_intrinsics)
{
MSG("Cannot compute icam_extrinsics. I don't have a pure calibration problem: %s observation %d has icam_intrinsics,icam_extrinsics %d,%d while I saw %d,%d previously",
what, i,
icam_map_to_intrinsics[icam_extrinsics+1], icam_extrinsics,
icam_intrinsics, icam_extrinsics);
return false;
}
if(icam_map_to_extrinsics[icam_intrinsics] == -100)
icam_map_to_extrinsics[icam_intrinsics] = icam_extrinsics;
else if(icam_map_to_extrinsics[icam_intrinsics] != icam_extrinsics)
{
MSG("Cannot compute icam_extrinsics. I don't have a pure calibration problem: %s observation %d has icam_intrinsics,icam_extrinsics %d,%d while I saw %d,%d previously",
what, i,
icam_intrinsics, icam_map_to_extrinsics[icam_intrinsics],
icam_intrinsics, icam_extrinsics);
return false;
}
return true;
}
for(int i=0; i<Nobservations_board; i++)
if(!check( &observations_board[i].icam, i, "board"))
return false;
for(int i=0; i<Nobservations_point; i++)
if(!check( &observations_point[i].icam, i, "point"))
return false;
*icam_extrinsics = icam_map_to_extrinsics[icam_intrinsics];
return true;
}
// Doing this myself instead of hooking into the logic in libdogleg for now.
// Bring back the fancy libdogleg logic once everything stabilizes
static
bool markOutliers(// output, input
// the weight stored in each mrcal_point3_t.z indicates outlierness
// on entry AND on exit. Outliers have weight < 0.0
mrcal_point3_t* observations_board_pool,
// output
int* Noutliers,
// input
const mrcal_observation_board_t* observations_board,
int Nobservations_board,
int calibration_object_width_n,
int calibration_object_height_n,
const double* x_measurements,
double observed_pixel_uncertainty,
bool verbose)
{
// I define an outlier as a feature that's > k stdevs past the mean. I make
// a broad assumption that the error distribution is normally-distributed,
// with mean 0. This is reasonable because this function is applied after
// running the optimization.
//
// The threshold stdev is the larger of
//
// - The stdev of my observed residuals
// - The expected stdev of my noise passed-in as the
// observed_pixel_uncertainty
//
// The rationale:
//
// - If I have a really good solve, the stdev of my data set will be very
// low, so deviations off that already-very-good solve aren't important. I
// use the expected-noise stdev in this case
//
// - If the solve isn't great, my errors may exceed the expected-noise stdev
// (if my model doesn't fit very well, say). In that case I want to use
// the stdev from the data
//
// This assumes that the given obsered_pixel_uncertainty isn't very
// well-known
//
// I have two separate thresholds. If any measurements are worse than the
// higher threshold, then I will need to reoptimize, so I throw out some
// extra points: all points worse than the lower threshold. This serves to
// reduce the required re-optimizations
// threshold. +- 3sigma includes 99.7% of the data in a normal distribution
const double k0 = 3.0;
const double k1 = 3.5;
*Noutliers = 0;
int i_pt,i_feature;
#define LOOP_FEATURE_BEGIN() \
i_feature = 0; \
for(int i_observation_board=0; \
i_observation_board<Nobservations_board; \
i_observation_board++) \
{ \
const mrcal_observation_board_t* observation = &observations_board[i_observation_board]; \
const int icam_intrinsics = observation->icam.intrinsics; \
for(i_pt=0; \
i_pt < calibration_object_width_n*calibration_object_height_n; \
i_pt++, i_feature++) \
{ \
const mrcal_point3_t* pt_observed = &observations_board_pool[i_feature]; \
double* weight = &observations_board_pool[i_feature].z;
#define LOOP_FEATURE_END() \
}}
double sum_weight = 0.0;
double var = 0.0;
LOOP_FEATURE_BEGIN()
{
if(*weight < 0.0)
{
(*Noutliers)++;
continue;
}
double dx = x_measurements[2*i_feature + 0];
double dy = x_measurements[2*i_feature + 1];
var += *weight * (dx*dx + dy*dy);
sum_weight += *weight;
}
LOOP_FEATURE_END();
var /= (2.*sum_weight);
var = fmax(var, observed_pixel_uncertainty*observed_pixel_uncertainty);
bool markedAny = false;
LOOP_FEATURE_BEGIN()
{
if(*weight < 0.0)
continue;
double dx = x_measurements[2*i_feature + 0];
double dy = x_measurements[2*i_feature + 1];
if(dx*dx > k1*k1*var ||
dy*dy > k1*k1*var )
{
*weight = -1.0;
markedAny = true;
(*Noutliers)++;
// MSG_IF_VERBOSE("Feature %d looks like an outlier. x/y are %f/%f stdevs off mean. Observed stdev: %f, limit: %f",
// i_feature, dx/sqrt(var), dy/sqrt(var), sqrt(var), k1);
}
}
LOOP_FEATURE_END();
if(!markedAny)
return false;
// Some measurements were past the worse threshold, so I throw out a bit
// extra to leave some margin so that the next re-optimization would be the
// last. Hopefully
LOOP_FEATURE_BEGIN()
{
if(*weight < 0)
continue;
double dx = x_measurements[2*i_feature + 0];
double dy = x_measurements[2*i_feature + 1];
if(dx*dx > k0*k0*var ||
dy*dy > k0*k0*var )
{
*weight *= -1.0;
(*Noutliers)++;
// MSG("Feature %d looks like an outlier. x/y are %f/%f stdevs off mean. Observed stdev: %f, limit: %f",
// i_feature, dx/sqrt(var), dy/sqrt(var), sqrt(var), k0);
}
}
LOOP_FEATURE_END();
return true;
#undef LOOP_FEATURE_BEGIN
#undef LOOP_FEATURE_END
}
typedef struct
{
// these are all UNPACKED
const double* intrinsics; // Ncameras_intrinsics * NlensParams of these
const mrcal_pose_t* extrinsics_fromref; // Ncameras_extrinsics of these. Transform FROM the reference frame
const mrcal_pose_t* frames_toref; // Nframes of these. Transform TO the reference frame
const mrcal_point3_t* points; // Npoints of these. In the reference frame
const mrcal_point2_t* calobject_warp; // 1 of these. May be NULL if !problem_details.do_optimize_calobject_warp
// in
int Ncameras_intrinsics, Ncameras_extrinsics, Nframes;
int Npoints, Npoints_fixed;
const mrcal_observation_board_t* observations_board;
const mrcal_point3_t* observations_board_pool;
int Nobservations_board;
const mrcal_observation_point_t* observations_point;
int Nobservations_point;
bool verbose;
mrcal_lensmodel_t lensmodel;
mrcal_projection_precomputed_t precomputed;
const int* imagersizes; // Ncameras_intrinsics*2 of these
mrcal_problem_details_t problem_details;
const mrcal_problem_constants_t* problem_constants;
double calibration_object_spacing;
int calibration_object_width_n;
int calibration_object_height_n;
const int Nmeasurements, N_j_nonzero, Nintrinsics;
const char* reportFitMsg;
} callback_context_t;
static
void optimizer_callback(// input state
const double* packed_state,
// output measurements
double* x,
// Jacobian
cholmod_sparse* Jt,
const callback_context_t* ctx)
{
double norm2_error = 0.0;
int iJacobian = 0;
int iMeasurement = 0;
int* Jrowptr = Jt ? (int*) Jt->p : NULL;
int* Jcolidx = Jt ? (int*) Jt->i : NULL;
double* Jval = Jt ? (double*)Jt->x : NULL;
#define STORE_JACOBIAN(col, g) \
do \
{ \
if(Jt) { \
Jcolidx[ iJacobian ] = col; \
Jval [ iJacobian ] = g; \
} \
iJacobian++; \
} while(0)
#define STORE_JACOBIAN2(col0, g0, g1) \
do \
{ \
if(Jt) { \
Jcolidx[ iJacobian+0 ] = col0+0; \
Jval [ iJacobian+0 ] = g0; \
Jcolidx[ iJacobian+1 ] = col0+1; \
Jval [ iJacobian+1 ] = g1; \
} \
iJacobian += 2; \
} while(0)
#define STORE_JACOBIAN3(col0, g0, g1, g2) \
do \
{ \
if(Jt) { \
Jcolidx[ iJacobian+0 ] = col0+0; \
Jval [ iJacobian+0 ] = g0; \
Jcolidx[ iJacobian+1 ] = col0+1; \
Jval [ iJacobian+1 ] = g1; \
Jcolidx[ iJacobian+2 ] = col0+2; \
Jval [ iJacobian+2 ] = g2; \
} \
iJacobian += 3; \
} while(0)
int Ncore = modelHasCore_fxfycxcy(ctx->lensmodel) ? 4 : 0;
int Ncore_state = (modelHasCore_fxfycxcy(ctx->lensmodel) &&
ctx->problem_details.do_optimize_intrinsics_core) ? 4 : 0;
// If I'm locking down some parameters, then the state vector contains a
// subset of my data. I reconstitute the intrinsics and extrinsics here.
// I do the frame poses later. This is a good way to do it if I have few
// cameras. With many cameras (this will be slow)
double intrinsics_all[ctx->Ncameras_intrinsics][ctx->Nintrinsics];
mrcal_pose_t camera_rt[ctx->Ncameras_extrinsics];
mrcal_point2_t calobject_warp_local = {};
const int i_var_calobject_warp =
mrcal_state_index_calobject_warp(ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
if(ctx->problem_details.do_optimize_calobject_warp)
unpack_solver_state_calobject_warp(&calobject_warp_local, &packed_state[i_var_calobject_warp]);
else if(ctx->calobject_warp != NULL)
calobject_warp_local = *ctx->calobject_warp;
for(int icam_intrinsics=0;
icam_intrinsics<ctx->Ncameras_intrinsics;
icam_intrinsics++)
{
// Construct the FULL intrinsics vector, based on either the
// optimization vector or the inputs, depending on what we're optimizing
double* intrinsics_here = &intrinsics_all[icam_intrinsics][0];
double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore];
int i_var_intrinsics =
mrcal_state_index_intrinsics(icam_intrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
if(Ncore)
{
if( ctx->problem_details.do_optimize_intrinsics_core )
{
intrinsics_here[0] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_FOCAL_LENGTH;
intrinsics_here[1] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_FOCAL_LENGTH;
intrinsics_here[2] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_CENTER_PIXEL;
intrinsics_here[3] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_CENTER_PIXEL;
}
else
memcpy( intrinsics_here,
&ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics],
Ncore*sizeof(double) );
}
if( ctx->problem_details.do_optimize_intrinsics_distortions )
{
for(int i = 0; i<ctx->Nintrinsics-Ncore; i++)
distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION;
}
else
memcpy( distortions_here,
&ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore],
(ctx->Nintrinsics-Ncore)*sizeof(double) );
}
for(int icam_extrinsics=0;
icam_extrinsics<ctx->Ncameras_extrinsics;
icam_extrinsics++)
{
if( icam_extrinsics < 0 ) continue;
const int i_var_camera_rt =
mrcal_state_index_extrinsics(icam_extrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
if(ctx->problem_details.do_optimize_extrinsics)
unpack_solver_state_extrinsics_one(&camera_rt[icam_extrinsics], &packed_state[i_var_camera_rt]);
else
memcpy(&camera_rt[icam_extrinsics], &ctx->extrinsics_fromref[icam_extrinsics], sizeof(mrcal_pose_t));
}
int i_feature = 0;
for(int i_observation_board = 0;
i_observation_board < ctx->Nobservations_board;
i_observation_board++)
{
const mrcal_observation_board_t* observation = &ctx->observations_board[i_observation_board];
const int icam_intrinsics = observation->icam.intrinsics;
const int icam_extrinsics = observation->icam.extrinsics;
const int iframe = observation->iframe;
// Some of these are bogus if problem_details says they're inactive
const int i_var_frame_rt =
mrcal_state_index_frames(iframe,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
mrcal_pose_t frame_rt;
if(ctx->problem_details.do_optimize_frames)
unpack_solver_state_framert_one(&frame_rt, &packed_state[i_var_frame_rt]);
else
memcpy(&frame_rt, &ctx->frames_toref[iframe], sizeof(mrcal_pose_t));
const int i_var_intrinsics =
mrcal_state_index_intrinsics(icam_intrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
// invalid if icam_extrinsics < 0, but unused in that case
const int i_var_camera_rt =
mrcal_state_index_extrinsics(icam_extrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
// these are computed in respect to the real-unit parameters,
// NOT the unit-scale parameters used by the optimizer
mrcal_point3_t dq_drcamera [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
mrcal_point3_t dq_dtcamera [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
mrcal_point3_t dq_drframe [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
mrcal_point3_t dq_dtframe [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
mrcal_point2_t dq_dcalobject_warp[ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
mrcal_point2_t q_hypothesis [ctx->calibration_object_width_n*ctx->calibration_object_height_n];
// I get the intrinsics gradients in separate arrays, possibly sparsely.
// All the data lives in dq_dintrinsics_pool_double[], with the other data
// indicating the meaning of the values in the pool.
//
// dq_dfxy serves a special-case for a perspective core. Such models
// are very common, and they have x = fx vx/vz + cx and y = fy vy/vz +
// cy. So x depends on fx and NOT on fy, and similarly for y. Similar
// for cx,cy, except we know the gradient value beforehand. I support
// this case explicitly here. I store dx/dfx and dy/dfy; no cross terms
double dq_dintrinsics_pool_double[ctx->calibration_object_width_n*ctx->calibration_object_height_n*2*(1+ctx->Nintrinsics)];
int dq_dintrinsics_pool_int [ctx->calibration_object_width_n*ctx->calibration_object_height_n];
double* dq_dfxy = NULL;
double* dq_dintrinsics_nocore = NULL;
gradient_sparse_meta_t gradient_sparse_meta = {};
int splined_intrinsics_grad_irun = 0;
project(q_hypothesis,
ctx->problem_details.do_optimize_intrinsics_core || ctx->problem_details.do_optimize_intrinsics_distortions ?
dq_dintrinsics_pool_double : NULL,
ctx->problem_details.do_optimize_intrinsics_core || ctx->problem_details.do_optimize_intrinsics_distortions ?
dq_dintrinsics_pool_int : NULL,
&dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta,
ctx->problem_details.do_optimize_extrinsics ?
(mrcal_point3_t*)dq_drcamera : NULL,
ctx->problem_details.do_optimize_extrinsics ?
(mrcal_point3_t*)dq_dtcamera : NULL,
ctx->problem_details.do_optimize_frames ?
(mrcal_point3_t*)dq_drframe : NULL,
ctx->problem_details.do_optimize_frames ?
(mrcal_point3_t*)dq_dtframe : NULL,
ctx->problem_details.do_optimize_calobject_warp ?
(mrcal_point2_t*)dq_dcalobject_warp : NULL,
// input
intrinsics_all[icam_intrinsics],
&camera_rt[icam_extrinsics], &frame_rt,
ctx->calobject_warp == NULL ? NULL : &calobject_warp_local,
icam_extrinsics < 0,
ctx->lensmodel, &ctx->precomputed,
ctx->calibration_object_spacing,
ctx->calibration_object_width_n,
ctx->calibration_object_height_n);
for(int i_pt=0;
i_pt < ctx->calibration_object_width_n*ctx->calibration_object_height_n;
i_pt++, i_feature++)
{
const mrcal_point3_t* qx_qy_w__observed = &ctx->observations_board_pool[i_feature];
double weight = qx_qy_w__observed->z;
if(weight >= 0.0)
{
// I have my two measurements (dx, dy). I propagate their
// gradient and store them
for( int i_xy=0; i_xy<2; i_xy++ )
{
const double err = (q_hypothesis[i_pt].xy[i_xy] - qx_qy_w__observed->xyz[i_xy]) * weight;
if( ctx->reportFitMsg )
{
MSG("%s: obs/frame/cam_i/cam_e/dot: %d %d %d %d %d err: %g",
ctx->reportFitMsg,
i_observation_board, iframe, icam_intrinsics, icam_extrinsics, i_pt, err);
continue;
}
if(Jt) Jrowptr[iMeasurement] = iJacobian;
x[iMeasurement] = err;
norm2_error += err*err;
if( ctx->problem_details.do_optimize_intrinsics_core )
{
// fx,fy. x depends on fx only. y depends on fy only
STORE_JACOBIAN( i_var_intrinsics + i_xy,
dq_dfxy[i_pt*2 + i_xy] *
weight * SCALE_INTRINSICS_FOCAL_LENGTH );
// cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only
STORE_JACOBIAN( i_var_intrinsics + i_xy+2,
weight * SCALE_INTRINSICS_CENTER_PIXEL );
}
if( ctx->problem_details.do_optimize_intrinsics_distortions )
{
if(gradient_sparse_meta.pool != NULL)
{
// u = stereographic(p)
// q = (u + deltau(u)) * f + c
//
// Intrinsics:
// dq/diii = f ddeltau/diii
//
// ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3])
const int ivar0 = dq_dintrinsics_pool_int[splined_intrinsics_grad_irun] -
( ctx->problem_details.do_optimize_intrinsics_core ? 0 : 4 );
const int len = gradient_sparse_meta.run_side_length;
const double* ABCDx = &gradient_sparse_meta.pool[len*2*splined_intrinsics_grad_irun + 0];
const double* ABCDy = &gradient_sparse_meta.pool[len*2*splined_intrinsics_grad_irun + len];
const int ivar_stridey = gradient_sparse_meta.ivar_stridey;
const double* fxy = &intrinsics_all[icam_intrinsics][0];
for(int iy=0; iy<len; iy++)
for(int ix=0; ix<len; ix++)
STORE_JACOBIAN( i_var_intrinsics + ivar0 + iy*ivar_stridey + ix*2 + i_xy,
ABCDx[ix]*ABCDy[iy]*fxy[i_xy] *
weight * SCALE_DISTORTION );
}
else
{
for(int i=0; i<ctx->Nintrinsics-Ncore; i++)
STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i,
dq_dintrinsics_nocore[i_pt*2*(ctx->Nintrinsics-Ncore) +
i_xy*(ctx->Nintrinsics-Ncore) +
i] *
weight * SCALE_DISTORTION );
}
}
if( ctx->problem_details.do_optimize_extrinsics )
if( icam_extrinsics >= 0 )
{
STORE_JACOBIAN3( i_var_camera_rt + 0,
dq_drcamera[i_pt][i_xy].xyz[0] *
weight * SCALE_ROTATION_CAMERA,
dq_drcamera[i_pt][i_xy].xyz[1] *
weight * SCALE_ROTATION_CAMERA,
dq_drcamera[i_pt][i_xy].xyz[2] *
weight * SCALE_ROTATION_CAMERA);
STORE_JACOBIAN3( i_var_camera_rt + 3,
dq_dtcamera[i_pt][i_xy].xyz[0] *
weight * SCALE_TRANSLATION_CAMERA,
dq_dtcamera[i_pt][i_xy].xyz[1] *
weight * SCALE_TRANSLATION_CAMERA,
dq_dtcamera[i_pt][i_xy].xyz[2] *
weight * SCALE_TRANSLATION_CAMERA);
}
if( ctx->problem_details.do_optimize_frames )
{
STORE_JACOBIAN3( i_var_frame_rt + 0,
dq_drframe[i_pt][i_xy].xyz[0] *
weight * SCALE_ROTATION_FRAME,
dq_drframe[i_pt][i_xy].xyz[1] *
weight * SCALE_ROTATION_FRAME,
dq_drframe[i_pt][i_xy].xyz[2] *
weight * SCALE_ROTATION_FRAME);
STORE_JACOBIAN3( i_var_frame_rt + 3,
dq_dtframe[i_pt][i_xy].xyz[0] *
weight * SCALE_TRANSLATION_FRAME,
dq_dtframe[i_pt][i_xy].xyz[1] *
weight * SCALE_TRANSLATION_FRAME,
dq_dtframe[i_pt][i_xy].xyz[2] *
weight * SCALE_TRANSLATION_FRAME);
}
if( ctx->problem_details.do_optimize_calobject_warp )
{
STORE_JACOBIAN2( i_var_calobject_warp,
dq_dcalobject_warp[i_pt][i_xy].x * weight * SCALE_CALOBJECT_WARP,
dq_dcalobject_warp[i_pt][i_xy].y * weight * SCALE_CALOBJECT_WARP);
}
iMeasurement++;
}
}
else
{
// Outlier.
// This is arbitrary. I'm skipping this observation, so I don't
// touch the projection results, and I set the measurement and
// all its gradients to 0. I need to have SOME dependency on the
// frame parameters to ensure a full-rank Hessian, so if we're
// skipping all observations for this frame the system will
// become singular. I don't currently handle this. libdogleg
// will complain loudly, and add small diagonal L2
// regularization terms
for( int i_xy=0; i_xy<2; i_xy++ )
{
const double err = 0.0;
if( ctx->reportFitMsg )
{
MSG( "%s: obs/frame/cam_i/cam_e/dot: %d %d %d %d %d err: %g",
ctx->reportFitMsg,
i_observation_board, iframe, icam_intrinsics, icam_extrinsics, i_pt, err);
continue;
}
if(Jt) Jrowptr[iMeasurement] = iJacobian;
x[iMeasurement] = err;
norm2_error += err*err;
if( ctx->problem_details.do_optimize_intrinsics_core )
{
STORE_JACOBIAN( i_var_intrinsics + i_xy, 0.0 );
STORE_JACOBIAN( i_var_intrinsics + i_xy+2, 0.0 );
}
if( ctx->problem_details.do_optimize_intrinsics_distortions )
{
if(gradient_sparse_meta.pool != NULL)
{
const int ivar0 = dq_dintrinsics_pool_int[splined_intrinsics_grad_irun] -
( ctx->problem_details.do_optimize_intrinsics_core ? 0 : 4 );
const int len = gradient_sparse_meta.run_side_length;
const int ivar_stridey = gradient_sparse_meta.ivar_stridey;
for(int iy=0; iy<len; iy++)
for(int ix=0; ix<len; ix++)
STORE_JACOBIAN( i_var_intrinsics + ivar0 + iy*ivar_stridey + ix*2 + i_xy, 0.0 );
}
else
{
for(int i=0; i<ctx->Nintrinsics-Ncore; i++)
STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, 0.0 );
}
}
if( ctx->problem_details.do_optimize_extrinsics )
if( icam_extrinsics >= 0 )
{
STORE_JACOBIAN3( i_var_camera_rt + 0, 0.0, 0.0, 0.0);
STORE_JACOBIAN3( i_var_camera_rt + 3, 0.0, 0.0, 0.0);
}
if( ctx->problem_details.do_optimize_frames )
{
// Arbitrary differences between the dimensions to keep
// my Hessian non-singular. This is 100% arbitrary. I'm
// skipping these measurements so these variables
// actually don't affect the computation at all
STORE_JACOBIAN3( i_var_frame_rt + 0, 0,0,0);
STORE_JACOBIAN3( i_var_frame_rt + 3, 0,0,0);
}
if( ctx->problem_details.do_optimize_calobject_warp )
STORE_JACOBIAN2( i_var_calobject_warp, 0.0, 0.0 );
iMeasurement++;
}
}
if(gradient_sparse_meta.pool != NULL)
splined_intrinsics_grad_irun++;
}
}
// Handle all the point observations. This is VERY similar to the
// board-observation loop above. Please consolidate
for(int i_observation_point = 0;
i_observation_point < ctx->Nobservations_point;
i_observation_point++)
{
const mrcal_observation_point_t* observation = &ctx->observations_point[i_observation_point];
const int icam_intrinsics = observation->icam.intrinsics;
const int icam_extrinsics = observation->icam.extrinsics;
const int i_point = observation->i_point;
const bool use_position_from_state =
ctx->problem_details.do_optimize_frames &&
i_point < ctx->Npoints - ctx->Npoints_fixed;
const mrcal_point3_t* qx_qy_w__observed = &observation->px;
double weight = qx_qy_w__observed->z;
if(weight < 0.0)
{
// Outlier. Cost = 0. Jacobians are 0 too, but I must preserve the
// structure
const int i_var_intrinsics =
mrcal_state_index_intrinsics(icam_intrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
// invalid if icam_extrinsics < 0, but unused in that case
const int i_var_camera_rt =
mrcal_state_index_extrinsics(icam_extrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
const int i_var_point =
mrcal_state_index_points(i_point,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
// I have my two measurements (dx, dy). I propagate their
// gradient and store them
for( int i_xy=0; i_xy<2; i_xy++ )
{
if(Jt) Jrowptr[iMeasurement] = iJacobian;
x[iMeasurement] = 0;
if( ctx->problem_details.do_optimize_intrinsics_core )
{
// fx,fy. x depends on fx only. y depends on fy only
STORE_JACOBIAN( i_var_intrinsics + i_xy, 0 );
// cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only
STORE_JACOBIAN( i_var_intrinsics + i_xy+2, 0);
}
if( ctx->problem_details.do_optimize_intrinsics_distortions )
{
if( (ctx->problem_details.do_optimize_intrinsics_core || ctx->problem_details.do_optimize_intrinsics_distortions) &&
ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC )
{
// sparse gradient. This is an outlier, so it doesn't
// matter which points I say I depend on, as long as I
// pick the right number, and says that j=0. I pick the
// control points at the start because why not
const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config =
&ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config;
int len = config->order+1;
for(int i=0; i<len*len; i++)
STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, 0);
}
else
for(int i=0; i<ctx->Nintrinsics-Ncore; i++)
STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, 0);
}
if(icam_extrinsics >= 0 && ctx->problem_details.do_optimize_extrinsics )
{
STORE_JACOBIAN3( i_var_camera_rt + 0, 0,0,0 );
STORE_JACOBIAN3( i_var_camera_rt + 3, 0,0,0 );
}
if( use_position_from_state )
STORE_JACOBIAN3( i_var_point, 0,0,0 );
iMeasurement++;
}
if(Jt) Jrowptr[iMeasurement] = iJacobian;
x[iMeasurement] = 0;
if(icam_extrinsics >= 0 && ctx->problem_details.do_optimize_extrinsics )
{
STORE_JACOBIAN3( i_var_camera_rt + 0, 0,0,0 );
STORE_JACOBIAN3( i_var_camera_rt + 3, 0,0,0 );
}
if( use_position_from_state )
STORE_JACOBIAN3( i_var_point, 0,0,0 );
iMeasurement++;
continue;
}
const int i_var_intrinsics =
mrcal_state_index_intrinsics(icam_intrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
// invalid if icam_extrinsics < 0, but unused in that case
const int i_var_camera_rt =
mrcal_state_index_extrinsics(icam_extrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
const int i_var_point =
mrcal_state_index_points(i_point,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
mrcal_point3_t point_ref;
if(use_position_from_state)
unpack_solver_state_point_one(&point_ref, &packed_state[i_var_point]);
else
point_ref = ctx->points[i_point];
#warning "compute size(dq_dintrinsics_pool_double) correctly and maybe bounds-check"
double dq_dintrinsics_pool_double[2*(1+ctx->Nintrinsics)];
int dq_dintrinsics_pool_int [1];
double* dq_dfxy = NULL;
double* dq_dintrinsics_nocore = NULL;
gradient_sparse_meta_t gradient_sparse_meta = {};
mrcal_point3_t dq_drcamera[2];
mrcal_point3_t dq_dtcamera[2];
mrcal_point3_t dq_dpoint [2];
// The array reference [-3] is intended, but the compiler throws a
// warning. I silence it here
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Warray-bounds"
mrcal_point2_t q_hypothesis;
project(&q_hypothesis,
ctx->problem_details.do_optimize_intrinsics_core || ctx->problem_details.do_optimize_intrinsics_distortions ?
dq_dintrinsics_pool_double : NULL,
ctx->problem_details.do_optimize_intrinsics_core || ctx->problem_details.do_optimize_intrinsics_distortions ?
dq_dintrinsics_pool_int : NULL,
&dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta,
ctx->problem_details.do_optimize_extrinsics ?
dq_drcamera : NULL,
ctx->problem_details.do_optimize_extrinsics ?
dq_dtcamera : NULL,
NULL, // frame rotation. I only have a point position
use_position_from_state ? dq_dpoint : NULL,
NULL,
// input
intrinsics_all[icam_intrinsics],
&camera_rt[icam_extrinsics],
// I only have the point position, so the 'rt' memory
// points 3 back. The fake "r" here will not be
// referenced
(mrcal_pose_t*)(&point_ref.xyz[-3]),
NULL,
icam_extrinsics < 0,
ctx->lensmodel, &ctx->precomputed,
0,0,0);
#pragma GCC diagnostic pop
// I have my two measurements (dx, dy). I propagate their
// gradient and store them
for( int i_xy=0; i_xy<2; i_xy++ )
{
const double err = (q_hypothesis.xy[i_xy] - qx_qy_w__observed->xyz[i_xy])*weight;
if(Jt) Jrowptr[iMeasurement] = iJacobian;
x[iMeasurement] = err;
norm2_error += err*err;
if( ctx->problem_details.do_optimize_intrinsics_core )
{
// fx,fy. x depends on fx only. y depends on fy only
STORE_JACOBIAN( i_var_intrinsics + i_xy,
dq_dfxy[i_xy] *
weight * SCALE_INTRINSICS_FOCAL_LENGTH );
// cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only
STORE_JACOBIAN( i_var_intrinsics + i_xy+2,
weight * SCALE_INTRINSICS_CENTER_PIXEL );
}
if( ctx->problem_details.do_optimize_intrinsics_distortions )
{
if(gradient_sparse_meta.pool != NULL)
{
// u = stereographic(p)
// q = (u + deltau(u)) * f + c
//
// Intrinsics:
// dq/diii = f ddeltau/diii
//
// ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3])
const int ivar0 = dq_dintrinsics_pool_int[0] -
( ctx->problem_details.do_optimize_intrinsics_core ? 0 : 4 );
const int len = gradient_sparse_meta.run_side_length;
const double* ABCDx = &gradient_sparse_meta.pool[0];
const double* ABCDy = &gradient_sparse_meta.pool[len];
const int ivar_stridey = gradient_sparse_meta.ivar_stridey;
const double* fxy = &intrinsics_all[icam_intrinsics][0];
for(int iy=0; iy<len; iy++)
for(int ix=0; ix<len; ix++)
{
STORE_JACOBIAN( i_var_intrinsics + ivar0 + iy*ivar_stridey + ix*2 + i_xy,
ABCDx[ix]*ABCDy[iy]*fxy[i_xy] *
weight * SCALE_DISTORTION );
}
}
else
{
for(int i=0; i<ctx->Nintrinsics-Ncore; i++)
STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i,
dq_dintrinsics_nocore[i_xy*(ctx->Nintrinsics-Ncore) +
i] *
weight * SCALE_DISTORTION );
}
}
if( ctx->problem_details.do_optimize_extrinsics )
if( icam_extrinsics >= 0 )
{
STORE_JACOBIAN3( i_var_camera_rt + 0,
dq_drcamera[i_xy].xyz[0] *
weight * SCALE_ROTATION_CAMERA,
dq_drcamera[i_xy].xyz[1] *
weight * SCALE_ROTATION_CAMERA,
dq_drcamera[i_xy].xyz[2] *
weight * SCALE_ROTATION_CAMERA);
STORE_JACOBIAN3( i_var_camera_rt + 3,
dq_dtcamera[i_xy].xyz[0] *
weight * SCALE_TRANSLATION_CAMERA,
dq_dtcamera[i_xy].xyz[1] *
weight * SCALE_TRANSLATION_CAMERA,
dq_dtcamera[i_xy].xyz[2] *
weight * SCALE_TRANSLATION_CAMERA);
}
if( use_position_from_state )
STORE_JACOBIAN3( i_var_point,
dq_dpoint[i_xy].xyz[0] *
weight * SCALE_POSITION_POINT,
dq_dpoint[i_xy].xyz[1] *
weight * SCALE_POSITION_POINT,
dq_dpoint[i_xy].xyz[2] *
weight * SCALE_POSITION_POINT);
iMeasurement++;
}
// Now the range normalization (make sure the range isn't
// aphysically high or aphysically low). This code is copied from
// project(). PLEASE consolidate
void get_penalty(// out
double* penalty, double* dpenalty_ddistsq,
// in
// SIGNED distance. <0 means "behind the camera"
const double distsq)
{
const double maxsq = ctx->problem_constants->point_max_range*ctx->problem_constants->point_max_range;
if(distsq > maxsq)
{
*penalty = weight * (distsq/maxsq - 1.0);
*dpenalty_ddistsq = weight*(1. / maxsq);
return;
}
const double minsq = ctx->problem_constants->point_min_range*ctx->problem_constants->point_min_range;
if(distsq < minsq)
{
// too close OR behind the camera
*penalty = weight*(1.0 - distsq/minsq);
*dpenalty_ddistsq = weight*(-1. / minsq);
return;
}
*penalty = *dpenalty_ddistsq = 0.0;
}
if(icam_extrinsics < 0)
{
double distsq =
point_ref.x*point_ref.x +
point_ref.y*point_ref.y +
point_ref.z*point_ref.z;
double penalty, dpenalty_ddistsq;
if(model_supports_projection_behind_camera(ctx->lensmodel) ||
point_ref.z > 0.0)
get_penalty(&penalty, &dpenalty_ddistsq, distsq);
else
{
get_penalty(&penalty, &dpenalty_ddistsq, -distsq);
dpenalty_ddistsq *= -1.;
}
if(Jt) Jrowptr[iMeasurement] = iJacobian;
x[iMeasurement] = penalty;
norm2_error += penalty*penalty;
if( use_position_from_state )
{
double scale = 2.0 * dpenalty_ddistsq * SCALE_POSITION_POINT;
STORE_JACOBIAN3( i_var_point,
scale*point_ref.x,
scale*point_ref.y,
scale*point_ref.z );
}
iMeasurement++;
}
else
{
// I need to transform the point. I already computed
// this stuff in project()...
double Rc[3*3];
double d_Rc_rc[9*3];
mrcal_R_from_r(Rc,
d_Rc_rc,
camera_rt[icam_extrinsics].r.xyz);
mrcal_point3_t pcam;
mul_vec3_gen33t_vout(point_ref.xyz, Rc, pcam.xyz);
add_vec(3, pcam.xyz, camera_rt[icam_extrinsics].t.xyz);
double distsq =
pcam.x*pcam.x +
pcam.y*pcam.y +
pcam.z*pcam.z;
double penalty, dpenalty_ddistsq;
if(model_supports_projection_behind_camera(ctx->lensmodel) ||
pcam.z > 0.0)
get_penalty(&penalty, &dpenalty_ddistsq, distsq);
else
{
get_penalty(&penalty, &dpenalty_ddistsq, -distsq);
dpenalty_ddistsq *= -1.;
}
if(Jt) Jrowptr[iMeasurement] = iJacobian;
x[iMeasurement] = penalty;
norm2_error += penalty*penalty;
if( ctx->problem_details.do_optimize_extrinsics )
{
// pcam.x = Rc[row0]*point*SCALE + tc
// d(pcam.x)/dr = d(Rc[row0])/drc*point*SCALE
// d(Rc[row0])/drc is 3x3 matrix at &d_Rc_rc[0]
double d_ptcamx_dr[3];
double d_ptcamy_dr[3];
double d_ptcamz_dr[3];
mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*0], d_ptcamx_dr );
mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*1], d_ptcamy_dr );
mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*2], d_ptcamz_dr );
STORE_JACOBIAN3( i_var_camera_rt + 0,
SCALE_ROTATION_CAMERA*
2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[0] +
pcam.y*d_ptcamy_dr[0] +
pcam.z*d_ptcamz_dr[0] ),
SCALE_ROTATION_CAMERA*
2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[1] +
pcam.y*d_ptcamy_dr[1] +
pcam.z*d_ptcamz_dr[1] ),
SCALE_ROTATION_CAMERA*
2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[2] +
pcam.y*d_ptcamy_dr[2] +
pcam.z*d_ptcamz_dr[2] ) );
STORE_JACOBIAN3( i_var_camera_rt + 3,
SCALE_TRANSLATION_CAMERA*
2.0*dpenalty_ddistsq*pcam.x,
SCALE_TRANSLATION_CAMERA*
2.0*dpenalty_ddistsq*pcam.y,
SCALE_TRANSLATION_CAMERA*
2.0*dpenalty_ddistsq*pcam.z );
}
if( use_position_from_state )
STORE_JACOBIAN3( i_var_point,
SCALE_POSITION_POINT*
2.0*dpenalty_ddistsq*(pcam.x*Rc[0] + pcam.y*Rc[3] + pcam.z*Rc[6]),
SCALE_POSITION_POINT*
2.0*dpenalty_ddistsq*(pcam.x*Rc[1] + pcam.y*Rc[4] + pcam.z*Rc[7]),
SCALE_POSITION_POINT*
2.0*dpenalty_ddistsq*(pcam.x*Rc[2] + pcam.y*Rc[5] + pcam.z*Rc[8]) );
iMeasurement++;
}
}
// regularization terms for the intrinsics. I favor smaller distortion
// parameters
if(ctx->problem_details.do_apply_regularization &&
modelHasCore_fxfycxcy(ctx->lensmodel) &&
( ctx->problem_details.do_optimize_intrinsics_distortions ||
ctx->problem_details.do_optimize_intrinsics_core
))
{
// I want the total regularization cost to be low relative to the
// other contributions to the cost. And I want each set of
// regularization terms to weigh roughly the same. Let's say I want
// regularization to account for ~ .5% of the other error
// contributions:
//
// Nmeasurements_rest*normal_pixel_error_sq * 0.005/2. =
// Nmeasurements_regularization_distortion *normal_regularization_distortion_error_sq =
// Nmeasurements_regularization_centerpixel*normal_regularization_centerpixel_error_sq =
//
// normal_regularization_distortion_error_sq = (scale*normal_centerpixel_offset)^2
// normal_regularization_centerpixel_error_sq = (scale*normal_distortion_value )^2
//
// Regularization introduces a bias to the solution. The
// test-projection-uncertainty test measures it, and barfs if it is too
// high. The constants should be adjusted if that test fails.
const bool dump_regularizaton_details = false;
int Nmeasurements_regularization_distortion = ctx->Ncameras_intrinsics*(ctx->Nintrinsics-Ncore);
int Nmeasurements_regularization_centerpixel = ctx->Ncameras_intrinsics*2;
int Nmeasurements_nonregularization =
ctx->Nmeasurements -
Nmeasurements_regularization_distortion -
Nmeasurements_regularization_centerpixel;
double normal_pixel_error = 1.0;
double expected_total_pixel_error_sq =
(double)Nmeasurements_nonregularization *
normal_pixel_error *
normal_pixel_error;
if(dump_regularizaton_details)
MSG("expected_total_pixel_error_sq: %f", expected_total_pixel_error_sq);
double scale_regularization_distortion =
({
double normal_distortion_value = 2.0;
double expected_regularization_distortion_error_sq_noscale =
(double)Nmeasurements_regularization_distortion *
normal_distortion_value *
normal_distortion_value;
double scale_sq =
expected_total_pixel_error_sq * 0.005/2. / expected_regularization_distortion_error_sq_noscale;
if(dump_regularizaton_details)
MSG("expected_regularization_distortion_error_sq: %f", expected_regularization_distortion_error_sq_noscale*scale_sq);
sqrt(scale_sq);
});
double scale_regularization_centerpixel =
({
double normal_centerpixel_offset = 500.0;
double expected_regularization_centerpixel_error_sq_noscale =
(double)Nmeasurements_regularization_centerpixel *
normal_centerpixel_offset *
normal_centerpixel_offset;
double scale_sq =
expected_total_pixel_error_sq * 0.005/2. / expected_regularization_centerpixel_error_sq_noscale;
if(dump_regularizaton_details)
MSG("expected_regularization_centerpixel_error_sq: %f", expected_regularization_centerpixel_error_sq_noscale*scale_sq);
sqrt(scale_sq);
});
for(int icam_intrinsics=0; icam_intrinsics<ctx->Ncameras_intrinsics; icam_intrinsics++)
{
const int i_var_intrinsics =
mrcal_state_index_intrinsics(icam_intrinsics,
ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics,
ctx->Nframes,
ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board,
ctx->problem_details, ctx->lensmodel);
if( ctx->problem_details.do_optimize_intrinsics_distortions)
{
for(int j=0; j<ctx->Nintrinsics-Ncore; j++)
{
if(Jt) Jrowptr[iMeasurement] = iJacobian;
// This maybe should live elsewhere, but I put it here
// for now. Various distortion coefficients have
// different meanings, and should be regularized in
// different ways. Specific logic follows
double scale = scale_regularization_distortion;
if( MRCAL_LENSMODEL_IS_OPENCV(ctx->lensmodel.type) &&
ctx->lensmodel.type >= MRCAL_LENSMODEL_OPENCV8 &&
5 <= j && j <= 7 )
{
// The radial distortion in opencv is x_distorted =
// x*scale where r2 = norm2(xy - xyc) and
//
// scale = (1 + k0 r2 + k1 r4 + k4 r6)/(1 + k5 r2 + k6 r4 + k7 r6)
//
// Note that k2,k3 are tangential (NOT radial)
// distortion components. Note that the r6 factor in
// the numerator is only present for
// >=MRCAL_LENSMODEL_OPENCV5. Note that the denominator
// is only present for >= MRCAL_LENSMODEL_OPENCV8. The
// danger with a rational model is that it's
// possible to get into a situation where scale ~
// 0/0 ~ 1. This would have very poorly behaved
// derivatives. If all the rational coefficients are
// ~0, then the denominator is always ~1, and this
// problematic case can't happen. I favor that by
// regularizing the coefficients in the denominator
// more strongly
scale *= 5.;
}
double err = scale*intrinsics_all[icam_intrinsics][j+Ncore];
x[iMeasurement] = err;
norm2_error += err*err;
STORE_JACOBIAN( i_var_intrinsics + Ncore_state + j,
scale * SCALE_DISTORTION );
iMeasurement++;
if(dump_regularizaton_details)
MSG("regularization distortion: %g; norm2: %g", err, err*err);
}
}
if( ctx->problem_details.do_optimize_intrinsics_core)
{
// And another regularization term: optical center should be
// near the middle. This breaks the symmetry between moving the
// center pixel coords and pitching/yawing the camera.
double cx_target = 0.5 * (double)(ctx->imagersizes[icam_intrinsics*2 + 0] - 1);
double cy_target = 0.5 * (double)(ctx->imagersizes[icam_intrinsics*2 + 1] - 1);
double err = scale_regularization_centerpixel *
(intrinsics_all[icam_intrinsics][2] - cx_target);
x[iMeasurement] = err;
norm2_error += err*err;
if(Jt) Jrowptr[iMeasurement] = iJacobian;
STORE_JACOBIAN( i_var_intrinsics + 2,
scale_regularization_centerpixel * SCALE_INTRINSICS_CENTER_PIXEL );
iMeasurement++;
if(dump_regularizaton_details)
MSG("regularization center pixel off-center: %g; norm2: %g", err, err*err);
err = scale_regularization_centerpixel *
(intrinsics_all[icam_intrinsics][3] - cy_target);
x[iMeasurement] = err;
norm2_error += err*err;
if(Jt) Jrowptr[iMeasurement] = iJacobian;
STORE_JACOBIAN( i_var_intrinsics + 3,
scale_regularization_centerpixel * SCALE_INTRINSICS_CENTER_PIXEL );
iMeasurement++;
if(dump_regularizaton_details)
MSG("regularization center pixel off-center: %g; norm2: %g", err, err*err);
}
}
}
// required to indicate the end of the jacobian matrix
if( !ctx->reportFitMsg )
{
if(Jt) Jrowptr[iMeasurement] = iJacobian;
if(iMeasurement != ctx->Nmeasurements)
{
MSG("Assertion (iMeasurement == ctx->Nmeasurements) failed: (%d != %d)",
iMeasurement, ctx->Nmeasurements);
assert(0);
}
if(iJacobian != ctx->N_j_nonzero )
{
MSG("Assertion (iJacobian == ctx->N_j_nonzero ) failed: (%d != %d)",
iJacobian, ctx->N_j_nonzero);
assert(0);
}
// MSG_IF_VERBOSE("RMS: %g", sqrt(norm2_error / ((double)ctx>Nmeasurements / 2.0)));
}
}
bool mrcal_optimizer_callback(// out
// Each one of these output pointers may be NULL
// Shape (Nstate,)
double* p_packed,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_p_packed,
// Shape (Nmeasurements,)
double* x,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_x,
// output Jacobian. May be NULL if we don't need
// it. This is the unitless Jacobian, used by the
// internal optimization routines
cholmod_sparse* Jt,
// in
// intrinsics is a concatenation of the intrinsics core
// and the distortion params. The specific distortion
// parameters may vary, depending on lensmodel, so
// this is a variable-length structure
const double* intrinsics, // Ncameras_intrinsics * NlensParams
const mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM reference frame
const mrcal_pose_t* frames_toref, // Nframes of these. Transform TO reference frame
const mrcal_point3_t* points, // Npoints of these. In the reference frame
const mrcal_point2_t* calobject_warp, // 1 of these. May be NULL if !problem_details.do_optimize_calobject_warp
int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
int Npoints, int Npoints_fixed, // at the end of points[]
const mrcal_observation_board_t* observations_board,
// All the board pixel observations, in order.
// .x, .y are the pixel observations
// .z is the weight of the observation. Most of the
// weights are expected to be 1.0, which implies
// that the noise on the observation is gaussian,
// independent on x,y, and has standard deviation
// of observed_pixel_uncertainty.
// observed_pixel_uncertainty scales inversely with
// the weight
const mrcal_point3_t* observations_board_pool,
int Nobservations_board,
const mrcal_observation_point_t* observations_point,
int Nobservations_point,
bool verbose,
mrcal_lensmodel_t lensmodel,
double observed_pixel_uncertainty,
const int* imagersizes, // Ncameras_intrinsics*2 of these
mrcal_problem_details_t problem_details,
const mrcal_problem_constants_t* problem_constants,
double calibration_object_spacing,
int calibration_object_width_n,
int calibration_object_height_n)
{
bool result = false;
if(!modelHasCore_fxfycxcy(lensmodel))
problem_details.do_optimize_intrinsics_core = false;
if(!problem_details.do_optimize_intrinsics_core &&
!problem_details.do_optimize_intrinsics_distortions &&
!problem_details.do_optimize_extrinsics &&
!problem_details.do_optimize_frames &&
!problem_details.do_optimize_calobject_warp)
{
MSG("Not optimizing any of our variables!");
goto done;
}
if( calobject_warp == NULL && problem_details.do_optimize_calobject_warp )
{
MSG("ERROR: We're using the calibration object warp, so a value MUST be passed in.");
goto done;
}
const int Nstate = mrcal_num_states(Ncameras_intrinsics, Ncameras_extrinsics,
Nframes,
Npoints, Npoints_fixed, Nobservations_board,
problem_details,
lensmodel);
if( buffer_size_p_packed != Nstate*(int)sizeof(double) )
{
MSG("The buffer passed to fill-in p_packed has the wrong size. Needed exactly %d bytes, but got %d bytes",
Nstate*(int)sizeof(double),buffer_size_p_packed);
goto done;
}
int Nmeasurements = mrcal_num_measurements(Nobservations_board,
Nobservations_point,
calibration_object_width_n,
calibration_object_height_n,
Ncameras_intrinsics, Ncameras_extrinsics,
Nframes,
Npoints, Npoints_fixed,
problem_details,
lensmodel);
int Nintrinsics = mrcal_lensmodel_num_params(lensmodel);
int N_j_nonzero = _mrcal_num_j_nonzero(Nobservations_board,
Nobservations_point,
calibration_object_width_n,
calibration_object_height_n,
Ncameras_intrinsics, Ncameras_extrinsics,
Nframes,
Npoints, Npoints_fixed,
observations_board,
observations_point,
problem_details,
lensmodel);
if( buffer_size_x != Nmeasurements*(int)sizeof(double) )
{
MSG("The buffer passed to fill-in x has the wrong size. Needed exactly %d bytes, but got %d bytes",
Nmeasurements*(int)sizeof(double),buffer_size_x);
goto done;
}
const int Npoints_fromBoards =
Nobservations_board *
calibration_object_width_n*calibration_object_height_n;
#warning "Outliers only work with board observations for now. Point outliers could be implemented without a lot of trouble now"
const callback_context_t ctx = {
.intrinsics = intrinsics,
.extrinsics_fromref = extrinsics_fromref,
.frames_toref = frames_toref,
.points = points,
.calobject_warp = calobject_warp,
.Ncameras_intrinsics = Ncameras_intrinsics,
.Ncameras_extrinsics = Ncameras_extrinsics,
.Nframes = Nframes,
.Npoints = Npoints,
.Npoints_fixed = Npoints_fixed,
.observations_board = observations_board,
.observations_board_pool = observations_board_pool,
.Nobservations_board = Nobservations_board,
.observations_point = observations_point,
.Nobservations_point = Nobservations_point,
.verbose = verbose,
.lensmodel = lensmodel,
.imagersizes = imagersizes,
.problem_details = problem_details,
.problem_constants = problem_constants,
.calibration_object_spacing = calibration_object_spacing,
.calibration_object_width_n = calibration_object_width_n > 0 ? calibration_object_width_n : 0,
.calibration_object_height_n= calibration_object_height_n > 0 ? calibration_object_height_n : 0,
.Nmeasurements = Nmeasurements,
.N_j_nonzero = N_j_nonzero,
.Nintrinsics = Nintrinsics};
_mrcal_precompute_lensmodel_data((mrcal_projection_precomputed_t*)&ctx.precomputed, lensmodel);
pack_solver_state(p_packed,
lensmodel, intrinsics,
extrinsics_fromref,
frames_toref,
points,
calobject_warp,
problem_details,
Ncameras_intrinsics, Ncameras_extrinsics,
Nframes, Npoints-Npoints_fixed, Nstate);
optimizer_callback(p_packed, x, Jt, &ctx);
result = true;
done:
return result;
}
mrcal_stats_t
mrcal_optimize( // out
// Each one of these output pointers may be NULL
// Shape (Nstate,)
double* p_packed_final,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_p_packed_final,
// Shape (Nmeasurements,)
double* x_final,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_x_final,
// out, in
//
// if(_solver_context != NULL) then this is a persistent solver
// context. The context is NOT freed on exit.
// mrcal_free_context() should be called to release it
//
// if(*_solver_context != NULL), the given context is reused
// if(*_solver_context == NULL), a context is created, and
// returned here on exit
void** _solver_context,
// These are a seed on input, solution on output
// intrinsics is a concatenation of the intrinsics core and the
// distortion params. The specific distortion parameters may
// vary, depending on lensmodel, so this is a variable-length
// structure
double* intrinsics, // Ncameras_intrinsics * NlensParams
mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame
mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame
mrcal_point3_t* points, // Npoints of these. In the reference frame
mrcal_point2_t* calobject_warp, // 1 of these. May be NULL if !problem_details.do_optimize_calobject_warp
// All the board pixel observations, in order.
// .x, .y are the pixel observations
// .z is the weight of the observation. Most of the weights are
// expected to be 1.0, which implies that the noise on the
// observation has standard deviation of
// observed_pixel_uncertainty. observed_pixel_uncertainty scales
// inversely with the weight.
//
// z<0 indicates that this is an outlier. This is respected on
// input (even if !do_apply_outlier_rejection). New outliers are
// marked with z<0 on output, so this isn't const
mrcal_point3_t* observations_board_pool,
int Nobservations_board,
// in
int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
int Npoints, int Npoints_fixed, // at the end of points[]
const mrcal_observation_board_t* observations_board,
const mrcal_observation_point_t* observations_point,
int Nobservations_point,
bool check_gradient,
bool verbose,
// Whether to try to find NEW outliers. The outliers given on
// input are respected regardless
const bool do_apply_outlier_rejection,
mrcal_lensmodel_t lensmodel,
double observed_pixel_uncertainty,
const int* imagersizes, // Ncameras_intrinsics*2 of these
mrcal_problem_details_t problem_details,
const mrcal_problem_constants_t* problem_constants,
double calibration_object_spacing,
int calibration_object_width_n,
int calibration_object_height_n)
{
if( Nobservations_board > 0 )
{
if( problem_details.do_optimize_calobject_warp && calobject_warp == NULL )
{
MSG("ERROR: We're optimizing the calibration object warp, so a buffer with a seed MUST be passed in.");
return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0};
}
}
else
problem_details.do_optimize_calobject_warp = false;
if(!modelHasCore_fxfycxcy(lensmodel))
problem_details.do_optimize_intrinsics_core = false;
if(!problem_details.do_optimize_intrinsics_core &&
!problem_details.do_optimize_intrinsics_distortions &&
!problem_details.do_optimize_extrinsics &&
!problem_details.do_optimize_frames &&
!problem_details.do_optimize_calobject_warp)
{
MSG("Warning: Not optimizing any of our variables");
}
dogleg_parameters2_t dogleg_parameters;
dogleg_getDefaultParameters(&dogleg_parameters);
dogleg_parameters.dogleg_debug = verbose ? DOGLEG_DEBUG_VNLOG : 0;
#warning update these parameters
// These were derived empirically, seeking high accuracy, fast convergence
// and without serious concern for performance. I looked only at a single
// frame. Tweak them please
dogleg_parameters.Jt_x_threshold = 0;
dogleg_parameters.update_threshold = 1e-6;
dogleg_parameters.trustregion_threshold = 0;
dogleg_parameters.max_iterations = 300;
// dogleg_parameters.trustregion_decrease_factor = 0.1;
// dogleg_parameters.trustregion_decrease_threshold = 0.15;
// dogleg_parameters.trustregion_increase_factor = 4.0
// dogleg_parameters.trustregion_increase_threshold = 0.75;
const int Npoints_fromBoards =
Nobservations_board *
calibration_object_width_n*calibration_object_height_n;
#warning "outliers only work with board observations for now"
callback_context_t ctx = {
.intrinsics = intrinsics,
.extrinsics_fromref = extrinsics_fromref,
.frames_toref = frames_toref,
.points = points,
.calobject_warp = calobject_warp,
.Ncameras_intrinsics = Ncameras_intrinsics,
.Ncameras_extrinsics = Ncameras_extrinsics,
.Nframes = Nframes,
.Npoints = Npoints,
.Npoints_fixed = Npoints_fixed,
.observations_board = observations_board,
.observations_board_pool = observations_board_pool,
.Nobservations_board = Nobservations_board,
.observations_point = observations_point,
.Nobservations_point = Nobservations_point,
.verbose = verbose,
.lensmodel = lensmodel,
.imagersizes = imagersizes,
.problem_details = problem_details,
.problem_constants = problem_constants,
.calibration_object_spacing = calibration_object_spacing,
.calibration_object_width_n = calibration_object_width_n > 0 ? calibration_object_width_n : 0,
.calibration_object_height_n= calibration_object_height_n > 0 ? calibration_object_height_n : 0,
.Nmeasurements = mrcal_num_measurements(Nobservations_board,
Nobservations_point,
calibration_object_width_n,
calibration_object_height_n,
Ncameras_intrinsics, Ncameras_extrinsics,
Nframes,
Npoints, Npoints_fixed,
problem_details,
lensmodel),
.N_j_nonzero = _mrcal_num_j_nonzero(Nobservations_board,
Nobservations_point,
calibration_object_width_n,
calibration_object_height_n,
Ncameras_intrinsics, Ncameras_extrinsics,
Nframes,
Npoints, Npoints_fixed,
observations_board,
observations_point,
problem_details,
lensmodel),
.Nintrinsics = mrcal_lensmodel_num_params(lensmodel)};
_mrcal_precompute_lensmodel_data((mrcal_projection_precomputed_t*)&ctx.precomputed, lensmodel);
const int Nstate = mrcal_num_states(Ncameras_intrinsics, Ncameras_extrinsics,
Nframes,
Npoints, Npoints_fixed, Nobservations_board,
problem_details,
lensmodel);
if( p_packed_final != NULL &&
buffer_size_p_packed_final != Nstate*(int)sizeof(double) )
{
MSG("The buffer passed to fill-in p_packed_final has the wrong size. Needed exactly %d bytes, but got %d bytes",
Nstate*(int)sizeof(double),buffer_size_p_packed_final);
return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0};
}
if( x_final != NULL &&
buffer_size_x_final != ctx.Nmeasurements*(int)sizeof(double) )
{
MSG("The buffer passed to fill-in x_final has the wrong size. Needed exactly %d bytes, but got %d bytes",
ctx.Nmeasurements*(int)sizeof(double),buffer_size_x_final);
return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0};
}
dogleg_solverContext_t* solver_context = NULL;
// If I have a context already, I free it and create it anew later. Ideally
// I'd reuse it, but then I'd need to make sure it's valid and such. Too
// much work for now
if(_solver_context != NULL && *_solver_context != NULL)
dogleg_freeContext((dogleg_solverContext_t**)_solver_context);
if(verbose)
MSG("## Nmeasurements=%d, Nstate=%d", ctx.Nmeasurements, Nstate);
if(ctx.Nmeasurements <= Nstate)
{
MSG("WARNING: problem isn't overdetermined: Nmeasurements=%d, Nstate=%d. Solver may not converge, and if it does, the results aren't reliable. Add more constraints and/or regularization",
ctx.Nmeasurements, Nstate);
}
double packed_state[Nstate];
pack_solver_state(packed_state,
lensmodel, intrinsics,
extrinsics_fromref,
frames_toref,
points,
calobject_warp,
problem_details,
Ncameras_intrinsics, Ncameras_extrinsics,
Nframes, Npoints-Npoints_fixed, Nstate);
double norm2_error = -1.0;
mrcal_stats_t stats = {.rms_reproj_error__pixels = -1.0 };
if( !check_gradient )
{
stats.Noutliers = 0;
#warning "outliers for board observations only"
int Nfeatures =
Nobservations_board *
calibration_object_width_n *
calibration_object_height_n;
for(int i=0; i<Nfeatures; i++)
if(observations_board_pool[i].z < 0.0)
stats.Noutliers++;
if(verbose)
{
ctx.reportFitMsg = "Before";
#warning hook this up
// optimizer_callback(packed_state, NULL, NULL, &ctx);
}
ctx.reportFitMsg = NULL;
double outliernessScale = -1.0;
do
{
norm2_error = dogleg_optimize2(packed_state,
Nstate, ctx.Nmeasurements, ctx.N_j_nonzero,
(dogleg_callback_t*)&optimizer_callback, &ctx,
&dogleg_parameters,
&solver_context);
if(_solver_context != NULL)
*_solver_context = solver_context;
if(norm2_error < 0)
// libdogleg barfed. I quit out
goto done;
#if 0
// Not using dogleg_markOutliers() (for now?)
if(outliernessScale < 0.0 && verbose)
// These are for debug reporting
dogleg_reportOutliers(getConfidence,
&outliernessScale,
2, Npoints_fromBoards,
stats.Noutliers,
solver_context->beforeStep, solver_context);
#endif
} while( do_apply_outlier_rejection &&
markOutliers(observations_board_pool,
&stats.Noutliers,
observations_board,
Nobservations_board,
calibration_object_width_n,
calibration_object_height_n,
solver_context->beforeStep->x,
observed_pixel_uncertainty,
verbose) &&
({MSG("Threw out some outliers (have a total of %d now); going again", stats.Noutliers); true;}));
// Done. I have the final state. I spit it back out
unpack_solver_state( intrinsics, // Ncameras_intrinsics of these
extrinsics_fromref, // Ncameras_extrinsics of these
frames_toref, // Nframes of these
points, // Npoints of these
calobject_warp,
packed_state,
lensmodel,
problem_details,
Ncameras_intrinsics, Ncameras_extrinsics,
Nframes, Npoints-Npoints_fixed, Nstate);
if(verbose)
{
// Not using dogleg_markOutliers() (for now?)
#if 0
// These are for debug reporting
dogleg_reportOutliers(getConfidence,
&outliernessScale,
2, Npoints_fromBoards,
stats.Noutliers,
solver_context->beforeStep, solver_context);
#endif
ctx.reportFitMsg = "After";
#warning hook this up
// optimizer_callback(packed_state, NULL, NULL, &ctx);
if(problem_details.do_apply_regularization)
{
double norm2_err_regularization = 0;
int Nmeasurements_regularization =
Ncameras_intrinsics *
num_regularization_terms_percamera(problem_details,
lensmodel);
for(int i=0; i<Nmeasurements_regularization; i++)
{
double x = solver_context->beforeStep->x[ctx.Nmeasurements-1 - i];
norm2_err_regularization += x*x;
}
double norm2_err_nonregularization = norm2_error - norm2_err_regularization;
double ratio_regularization_cost = norm2_err_regularization / norm2_error;
// Disable this by default. Splined models have LOTS of
// parameters, and I don't want to print them. Usually.
//
// for(int i=0; i<Nmeasurements_regularization; i++)
// {
// double x = solver_context->beforeStep->x[ctx.Nmeasurements - Nmeasurements_regularization + i];
// MSG("regularization %d: %f (squared: %f)", i, x, x*x);
// }
MSG("norm2_error: %f", norm2_error);
MSG("norm2_err_regularization: %f", norm2_err_regularization);
MSG("regularization cost ratio: %g", ratio_regularization_cost);
}
}
}
else
for(int ivar=0; ivar<Nstate; ivar++)
dogleg_testGradient(ivar, packed_state,
Nstate, ctx.Nmeasurements, ctx.N_j_nonzero,
(dogleg_callback_t*)&optimizer_callback, &ctx);
stats.rms_reproj_error__pixels =
// /2 because I have separate x and y measurements
sqrt(norm2_error / ((double)ctx.Nmeasurements / 2.0));
if(p_packed_final)
memcpy(p_packed_final, solver_context->beforeStep->p, Nstate*sizeof(double));
if(x_final)
memcpy(x_final, solver_context->beforeStep->x, ctx.Nmeasurements*sizeof(double));
done:
if(_solver_context == NULL && solver_context)
dogleg_freeContext(&solver_context);
return stats;
}
// frees a dogleg_solverContext_t. I don't want to #include <dogleg.h> here, so
// this is void
void mrcal_free_context(void** ctx)
{
if( *ctx == NULL )
return;
dogleg_freeContext((dogleg_solverContext_t**)ctx);
}