From e58cb27492186a8863d0335891f98a3286263de5 Mon Sep 17 00:00:00 2001 From: Gold856 <117957790+Gold856@users.noreply.github.com> Date: Tue, 22 Oct 2024 02:39:47 -0400 Subject: [PATCH] Test --- upstream_utils/libdogleg.py | 19 +- upstream_utils/mrcal.py | 30 +- wpical/build.gradle | 15 + .../thirdparty/libdogleg/include/dogleg.h | 2 +- .../native/thirdparty/libdogleg/src/dogleg.c | 2 +- .../thirdparty/mrcal/include/autodiff.hh | 91 +- .../mrcal/include/minimath/minimath-extra.h | 22 - .../thirdparty/mrcal/include/mrcal-internal.h | 5 - .../thirdparty/mrcal/include/mrcal-types.h | 55 +- .../native/thirdparty/mrcal/include/mrcal.h | 139 +- .../thirdparty/mrcal/include/poseutils.h | 24 +- .../native/thirdparty/mrcal/include/scales.h | 43 - .../thirdparty/mrcal/include/triangulation.h | 22 - .../main/native/thirdparty/mrcal/src/mrcal.c | 1955 ++++------------- .../main/native/thirdparty/mrcal/src/opencv.c | 152 -- .../mrcal/src/poseutils-uses-autodiff.cpp | 403 ++-- .../native/thirdparty/mrcal/src/poseutils.c | 365 --- .../thirdparty/mrcal/src/triangulation.cpp | 356 +-- 18 files changed, 704 insertions(+), 2996 deletions(-) delete mode 100644 wpical/src/main/native/thirdparty/mrcal/include/scales.h delete mode 100644 wpical/src/main/native/thirdparty/mrcal/src/opencv.c diff --git a/upstream_utils/libdogleg.py b/upstream_utils/libdogleg.py index c567eefa3de..08472212741 100644 --- a/upstream_utils/libdogleg.py +++ b/upstream_utils/libdogleg.py @@ -16,14 +16,29 @@ def copy_upstream_src(wpilib_root): ]: shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) - walk_cwd_and_copy_if( + files = walk_cwd_and_copy_if( lambda dp, f: f.endswith("dogleg.h"), os.path.join(wpical, "src/main/native/thirdparty/libdogleg/include"), ) - walk_cwd_and_copy_if( + for f in files: + with open(f) as file: + content = file.read() + content = content.replace( + "#include ", "#include " + ) + with open(f, "w") as file: + file.write(content) + + files = walk_cwd_and_copy_if( lambda dp, f: f.endswith("dogleg.c"), os.path.join(wpical, "src/main/native/thirdparty/libdogleg/src"), ) + for f in files: + with open(f) as file: + content = file.read() + content = content.replace("#warning", "// #warning") + with open(f, "w") as file: + file.write(content) def main(): diff --git a/upstream_utils/mrcal.py b/upstream_utils/mrcal.py index 40a8e826d65..408d0256552 100644 --- a/upstream_utils/mrcal.py +++ b/upstream_utils/mrcal.py @@ -3,7 +3,7 @@ import os import shutil -from upstream_utils import Lib, walk_cwd_and_copy_if +from upstream_utils import Lib, comment_out_invalid_includes, walk_cwd_and_copy_if def copy_upstream_src(wpilib_root): @@ -16,14 +16,28 @@ def copy_upstream_src(wpilib_root): ]: shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) - walk_cwd_and_copy_if( + files = walk_cwd_and_copy_if( lambda dp, f: (f.endswith(".h") or f.endswith(".hh")) and not f.endswith("stereo.h") and not f.endswith("stereo-matching-libelas.h") and not dp.startswith(os.path.join(".", "test")), os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), ) - walk_cwd_and_copy_if( + for f in files: + comment_out_invalid_includes( + f, + [ + os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), + os.path.join(wpical, "src/main/native/thirdparty/mrcal/generated"), + ], + ) + # with open(f) as file: + # content = file.read() + # content = content.replace(r"typedef struct {}", r"// typedef struct {}") + # with open(f, "w") as file: + # file.write(content) + + files = walk_cwd_and_copy_if( lambda dp, f: (f.endswith(".c") or f.endswith(".cc") or f.endswith(".pl")) and not f.endswith("mrcal-pywrap.c") and not f.endswith("image.c") @@ -34,12 +48,20 @@ def copy_upstream_src(wpilib_root): and not dp.startswith(os.path.join(".", "test")), os.path.join(wpical, "src/main/native/thirdparty/mrcal/src"), ) + for f in files: + comment_out_invalid_includes( + f, + [ + os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), + os.path.join(wpical, "src/main/native/thirdparty/mrcal/generated"), + ], + ) def main(): name = "mrcal" url = "https://github.com/dkogan/mrcal" - tag = "71c89c4e9f268a0f4fb950325e7d551986a281ec" + tag = "0d5426b5851be80dd8e51470a0784a73565a3006" mrcal = Lib(name, url, tag, copy_upstream_src) mrcal.main() diff --git a/wpical/build.gradle b/wpical/build.gradle index ce17e8c07a7..eed66e3d78e 100644 --- a/wpical/build.gradle +++ b/wpical/build.gradle @@ -71,6 +71,21 @@ tasks.withType(CppCompile) { dependsOn generateCppVersion } +// Suppress sign-compare warnings +nativeUtils.platformConfigs.each { + if (it.name.contains('windows')) { + } else if (it.name.contains('osx')) { + it.cCompiler.args.add("-Wno-format-nonliteral") + it.cCompiler.args.add("-Wno-unused-function") + it.cCompiler.args.remove("-pedantic") + it.cppCompiler.args.add("-Wno-extern-c-compat") + it.cppCompiler.args.remove("-pedantic") + } else { + it.cCompiler.args.add("-Wno-format-nonliteral") + it.cppCompiler.args.add("-Wno-missing-field-initializers") + it.cppCompiler.args.remove("-pedantic") + } +} model { components { // By default, a development executable will be generated. This is to help the case of diff --git a/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h b/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h index 74337263a89..881fb9c4cc5 100644 --- a/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h +++ b/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h @@ -5,7 +5,7 @@ #pragma once -#include +#include #include typedef void (dogleg_callback_t)(const double* p, diff --git a/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.c b/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.c index 7e4259c303e..80205cbffdf 100644 --- a/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.c +++ b/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.c @@ -1812,7 +1812,7 @@ static void accum_outlierness_factor(// output } -#warning This is a hack. The threshold should be 1.0, and the scaling should make sure that is the case. I am leaving it for now +// #warning This is a hack. The threshold should be 1.0, and the scaling should make sure that is the case. I am leaving it for now k /= 8.; diff --git a/wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh b/wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh index b6f217354c9..62b4758cae7 100644 --- a/wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh +++ b/wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh @@ -28,13 +28,11 @@ struct val_withgrad_t double x; double j[NGRAD]; - __attribute__ ((visibility ("hidden"))) val_withgrad_t(double _x = 0.0) : x(_x) { for(int i=0; i operator+( const val_withgrad_t& b ) const { val_withgrad_t y = *this; @@ -43,19 +41,16 @@ struct val_withgrad_t y.j[i] += b.j[i]; return y; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t operator+( double b ) const { val_withgrad_t y = *this; y.x += b; return y; } - __attribute__ ((visibility ("hidden"))) void operator+=( const val_withgrad_t& b ) { *this = (*this) + b; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t operator-( const val_withgrad_t& b ) const { val_withgrad_t y = *this; @@ -64,24 +59,20 @@ struct val_withgrad_t y.j[i] -= b.j[i]; return y; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t operator-( double b ) const { val_withgrad_t y = *this; y.x -= b; return y; } - __attribute__ ((visibility ("hidden"))) void operator-=( const val_withgrad_t& b ) { *this = (*this) - b; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t operator-() const { return (*this) * (-1); } - __attribute__ ((visibility ("hidden"))) val_withgrad_t operator*( const val_withgrad_t& b ) const { val_withgrad_t y; @@ -90,7 +81,6 @@ struct val_withgrad_t y.j[i] = j[i]*b.x + x*b.j[i]; return y; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t operator*( double b ) const { val_withgrad_t y; @@ -99,17 +89,10 @@ struct val_withgrad_t y.j[i] = j[i]*b; return y; } - __attribute__ ((visibility ("hidden"))) void operator*=( const val_withgrad_t& b ) { *this = (*this) * b; } - __attribute__ ((visibility ("hidden"))) - void operator*=( const double b ) - { - *this = (*this) * b; - } - __attribute__ ((visibility ("hidden"))) val_withgrad_t operator/( const val_withgrad_t& b ) const { val_withgrad_t y; @@ -118,22 +101,11 @@ struct val_withgrad_t y.j[i] = (j[i]*b.x - x*b.j[i]) / (b.x*b.x); return y; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t operator/( double b ) const { return (*this) * (1./b); } - __attribute__ ((visibility ("hidden"))) - void operator/=( const val_withgrad_t& b ) - { - *this = (*this) / b; - } - __attribute__ ((visibility ("hidden"))) - void operator/=( const double b ) - { - *this = (*this) / b; - } - __attribute__ ((visibility ("hidden"))) + val_withgrad_t sqrt(void) const { val_withgrad_t y; @@ -143,7 +115,6 @@ struct val_withgrad_t return y; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t square(void) const { val_withgrad_t s; @@ -153,7 +124,6 @@ struct val_withgrad_t return s; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t sin(void) const { const double s = ::sin(x); @@ -165,7 +135,6 @@ struct val_withgrad_t return y; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t cos(void) const { const double s = ::sin(x); @@ -177,7 +146,6 @@ struct val_withgrad_t return y; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t sincos(void) const { const double s = ::sin(x); @@ -193,7 +161,6 @@ struct val_withgrad_t return sc; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t tan(void) const { const double s = ::sin(x); @@ -205,7 +172,6 @@ struct val_withgrad_t return y; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t atan2(val_withgrad_t& x) const { val_withgrad_t th; @@ -223,24 +189,6 @@ struct val_withgrad_t return th; } - // This function does NOT check for overflow. The gradient is infinite at - // the valid bounds of the input. So the caller has to do the right thing in - // those cases - __attribute__ ((visibility ("hidden"))) - val_withgrad_t asin(void) const - { - val_withgrad_t th; - th.x = ::asin(x); - double dasin_dx = 1. / ::sqrt( 1. - x*x ); - for(int i=0; i acos(void) const { val_withgrad_t th; @@ -251,7 +199,6 @@ struct val_withgrad_t return th; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t sinx_over_x(// To avoid recomputing it const val_withgrad_t& sinx) const { @@ -270,7 +217,7 @@ struct val_withgrad_t // 0 // // So for small x the gradient is 0 - if(fabs(x) < 1e-5) + if(fabs(x) < 1e-8) return val_withgrad_t(1.0); return sinx / (*this); @@ -285,7 +232,6 @@ struct vec_withgrad_t vec_withgrad_t() {} - __attribute__ ((visibility ("hidden"))) void init_vars(const double* x_in, int ivar0, int Nvars, int i_gradvec0 = -1, int stride = sizeof(double)) { @@ -305,31 +251,26 @@ struct vec_withgrad_t } } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t(const double* x_in, int i_gradvec0 = -1, int stride = sizeof(double)) { init_vars(x_in, 0, NVEC, i_gradvec0, stride); } - __attribute__ ((visibility ("hidden"))) val_withgrad_t& operator[](int i) { return v[i]; } - __attribute__ ((visibility ("hidden"))) const val_withgrad_t& operator[](int i) const { return v[i]; } - __attribute__ ((visibility ("hidden"))) void operator+=( const vec_withgrad_t& x ) { (*this) = (*this) + x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator+( const vec_withgrad_t& x ) const { vec_withgrad_t p; @@ -338,12 +279,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator+=( const val_withgrad_t& x ) { (*this) = (*this) + x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator+( const val_withgrad_t& x ) const { vec_withgrad_t p; @@ -352,12 +291,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator+=( double x ) { (*this) = (*this) + x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator+( double x ) const { vec_withgrad_t p; @@ -366,12 +303,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator-=( const vec_withgrad_t& x ) { (*this) = (*this) - x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator-( const vec_withgrad_t& x ) const { vec_withgrad_t p; @@ -380,12 +315,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator-=( const val_withgrad_t& x ) { (*this) = (*this) - x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator-( const val_withgrad_t& x ) const { vec_withgrad_t p; @@ -394,12 +327,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator-=( double x ) { (*this) = (*this) - x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator-( double x ) const { vec_withgrad_t p; @@ -408,12 +339,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator*=( const vec_withgrad_t& x ) { (*this) = (*this) * x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator*( const vec_withgrad_t& x ) const { vec_withgrad_t p; @@ -422,12 +351,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator*=( const val_withgrad_t& x ) { (*this) = (*this) * x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator*( const val_withgrad_t& x ) const { vec_withgrad_t p; @@ -436,12 +363,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator*=( double x ) { (*this) = (*this) * x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator*( double x ) const { vec_withgrad_t p; @@ -450,12 +375,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator/=( const vec_withgrad_t& x ) { (*this) = (*this) / x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator/( const vec_withgrad_t& x ) const { vec_withgrad_t p; @@ -464,12 +387,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator/=( const val_withgrad_t& x ) { (*this) = (*this) / x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator/( const val_withgrad_t& x ) const { vec_withgrad_t p; @@ -478,12 +399,10 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) void operator/=( double x ) { (*this) = (*this) / x; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t operator/( double x ) const { vec_withgrad_t p; @@ -492,7 +411,6 @@ struct vec_withgrad_t return p; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t dot( const vec_withgrad_t& x) const { val_withgrad_t d; // initializes to 0 @@ -504,7 +422,6 @@ struct vec_withgrad_t return d; } - __attribute__ ((visibility ("hidden"))) vec_withgrad_t cross( const vec_withgrad_t& x) const { vec_withgrad_t c; @@ -514,20 +431,17 @@ struct vec_withgrad_t return c; } - __attribute__ ((visibility ("hidden"))) val_withgrad_t norm2(void) const { return dot(*this); } - __attribute__ ((visibility ("hidden"))) val_withgrad_t mag(void) const { val_withgrad_t l2 = norm2(); return l2.sqrt(); } - __attribute__ ((visibility ("hidden"))) void extract_value(double* out, int stride = sizeof(double), int ivar0 = 0, int Nvars = NVEC) const @@ -535,7 +449,6 @@ struct vec_withgrad_t for(int i=ivar0; i=2 - // cameras can observe any point. All observations of a given point are - // stored consecutively, the last one being noted by this bit -#warning "triangulated-solve: do I really need this? I cannot look at the next observation to determine when this one is done?" - bool last_in_set : 1; - -#warning "triangulated-solve: this is temporary. Should be a weight in observations_point_pool like all the other observations" - bool outlier : 1; // Observed pixel coordinates. This works just like elements of - // observations_board_pool and observations_point_pool + // observations_board_pool: + // + // .x, .y are the pixel observations + // .z is the weight of the observation. Most of the weights are expected to + // be 1.0. Less precise observations have lower weights. + // .z<0 indicates that this is an outlier. This is respected on + // input + // + // Unlike observations_board_pool, outlier rejection is NOT YET IMPLEMENTED + // for points, so outlier points will NOT be found and reported on output in + // .z<0 mrcal_point3_t px; -} mrcal_observation_point_triangulated_t; - - -#warning "triangulated-solve: need a function to identify a vanilla calibration problem. It needs to not include any triangulated points. The noise propagation is different" - +} mrcal_observation_point_t; // Bits indicating which parts of the optimization problem being solved. We can // ask mrcal to solve for ALL the lens parameters and ALL the geometry and @@ -275,8 +257,6 @@ typedef struct // If true, optimize the shape of the calibration object bool do_optimize_calobject_warp : 1; -#warning "triangulated-solve: Need finer-grained regularization flags" -#warning "triangulated-solve: Regularization flags should reflect do_optimize stuff and Ncameras stuff" // If true, apply the regularization terms in the solver bool do_apply_regularization : 1; @@ -284,9 +264,6 @@ typedef struct // input are respected regardless bool do_apply_outlier_rejection : 1; - // Pull the distance between the first two cameras to 1.0 - bool do_apply_regularization_unity_cam01: 1; - } mrcal_problem_selections_t; // Constants used in a mrcal optimization. This is similar to @@ -319,13 +296,7 @@ typedef struct /* How many pixel observations were thrown out as outliers. Each pixel */ \ /* observation produces two measurements. Note that this INCLUDES any */ \ /* outliers that were passed-in at the start */ \ - _(int, Noutliers_board, PyLong_FromLong) \ - \ - /* How many pixel observations were thrown out as outliers. Each pixel */ \ - /* observation produces two measurements. Note that this INCLUDES any */ \ - /* outliers that were passed-in at the start */ \ - _(int, Noutliers_triangulated_point, PyLong_FromLong) -#warning "triangulated-solve: implement stats.Noutliers_triangulated_point; add to c-api.org" + _(int, Noutliers, PyLong_FromLong) #define MRCAL_STATS_ITEM_DEFINE(type, name, pyconverter) type name; typedef struct { diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h index ebb751eca30..773f287baaf 100644 --- a/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h @@ -13,7 +13,7 @@ #include "mrcal-types.h" #include "poseutils.h" -#include "stereo.h" +// #include "stereo.h" #include "triangulation.h" #include "mrcal-image.h" @@ -197,7 +197,7 @@ bool mrcal_unproject( // out // Project the given camera-coordinate-system points using a pinhole // model. See the docs for projection details: -// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole +// http://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole // // This is a simplified special case of mrcal_project(). We project N // camera-coordinate-system points p to N pixel coordinates q @@ -216,7 +216,7 @@ void mrcal_project_pinhole( // output // Unproject the given pixel coordinates using a pinhole model. // See the docs for projection details: -// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole +// http://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole // // This is a simplified special case of mrcal_unproject(). We unproject N 2D // pixel coordinates q to N camera-coordinate-system vectors v. The returned @@ -235,7 +235,7 @@ void mrcal_unproject_pinhole( // output // Project the given camera-coordinate-system points using a stereographic // model. See the docs for projection details: -// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic +// http://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic // // This is a simplified special case of mrcal_project(). We project N // camera-coordinate-system points p to N pixel coordinates q @@ -254,7 +254,7 @@ void mrcal_project_stereographic( // output // Unproject the given pixel coordinates using a stereographic model. // See the docs for projection details: -// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic +// http://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic // // This is a simplified special case of mrcal_unproject(). We unproject N 2D // pixel coordinates q to N camera-coordinate-system vectors v. The returned @@ -274,7 +274,7 @@ void mrcal_unproject_stereographic( // output // Project the given camera-coordinate-system points using an equirectangular // projection. See the docs for projection details: -// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat +// http://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat // // This is a simplified special case of mrcal_project(). We project N // camera-coordinate-system points p to N pixel coordinates q @@ -294,7 +294,7 @@ void mrcal_project_lonlat( // output // Unproject the given pixel coordinates using an equirectangular projection. // See the docs for projection details: -// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat +// http://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat // // This is a simplified special case of mrcal_unproject(). We unproject N 2D // pixel coordinates q to N camera-coordinate-system vectors v. The returned @@ -316,7 +316,7 @@ void mrcal_unproject_lonlat( // output // Project the given camera-coordinate-system points using a transverse // equirectangular projection. See the docs for projection details: -// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon +// http://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon // // This is a simplified special case of mrcal_project(). We project N // camera-coordinate-system points p to N pixel coordinates q @@ -336,7 +336,7 @@ void mrcal_project_latlon( // output // Unproject the given pixel coordinates using a transverse equirectangular // projection. See the docs for projection details: -// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon +// http://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon // // This is a simplified special case of mrcal_unproject(). We unproject N 2D // pixel coordinates q to N camera-coordinate-system vectors v. The returned @@ -361,6 +361,8 @@ void mrcal_unproject_latlon( // output //////////////////// Optimization //////////////////////////////////////////////////////////////////////////////// + + // Return the number of parameters needed in optimizing the given lens model // // This is identical to mrcal_lensmodel_num_params(), but takes into account the @@ -483,29 +485,22 @@ mrcal_optimize( // out int Nobservations_board, int Nobservations_point, - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - // All the board pixel observations, in an array of shape // // ( Nobservations_board, // calibration_object_height_n, // calibration_object_width_n ) // - // .x, .y are the pixel observations .z is the weight of the - // observation. Most of the weights are expected to be 1.0. Less - // precise observations have lower weights. + // .x, .y are the + // pixel observations .z is the weight of the observation. Most + // of the weights are expected to be 1.0. Less precise + // observations have lower weights. // // .z<0 indicates that this is an outlier. This is respected on // input (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, - // Same this, but for discrete points. Array of shape - // - // ( Nobservations_point,) - mrcal_point3_t* observations_point_pool, - const mrcal_lensmodel_t* lensmodel, const int* imagersizes, // Ncameras_intrinsics*2 of these mrcal_problem_selections_t problem_selections, @@ -518,12 +513,10 @@ mrcal_optimize( // out bool check_gradient); -// These are cholmod_sparse, cholmod_factor, cholmod_common. I don't want to -// include the full header that defines these in mrcal.h, and I don't need to: -// mrcal.h just needs to know that these are a structure +// This is cholmod_sparse. I don't want to include the full header that defines +// it in mrcal.h, and I don't need to: mrcal.h just needs to know that it's a +// structure struct cholmod_sparse_struct; -struct cholmod_factor_struct; -struct cholmod_common_struct; // Evaluate the value of the callback function at the given operating point // @@ -576,9 +569,6 @@ bool mrcal_optimizer_callback(// out int Nobservations_board, int Nobservations_point, - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - // All the board pixel observations, in an array of shape // // ( Nobservations_board, @@ -593,11 +583,6 @@ bool mrcal_optimizer_callback(// out // .z<0 indicates that this is an outlier const mrcal_point3_t* observations_board_pool, - // Same this, but for discrete points. Array of shape - // - // ( Nobservations_point,) - const mrcal_point3_t* observations_point_pool, - const mrcal_lensmodel_t* lensmodel, const int* imagersizes, // Ncameras_intrinsics*2 of these mrcal_problem_selections_t problem_selections, @@ -607,49 +592,6 @@ bool mrcal_optimizer_callback(// out int calibration_object_height_n, bool verbose); -bool mrcal_drt_ref_refperturbed__dbpacked(// output - // Shape (6,Nstate_frames) - double* Kpackedf, - int Kpackedf_stride0, // in bytes. <= 0 means "contiguous" - int Kpackedf_stride1, // in bytes. <= 0 means "contiguous" - - // Shape (6,Nstate_points) - double* Kpackedp, - int Kpackedp_stride0, // in bytes. <= 0 means "contiguous" - int Kpackedp_stride1, // in bytes. <= 0 means "contiguous" - - // Shape (6,Nstate_calobject_warp) - double* Kpackedcw, - int Kpackedcw_stride0, // in bytes. <= 0 means "contiguous" - int Kpackedcw_stride1, // in bytes. <= 0 means "contiguous" - - // inputs - // stuff that describes this solve - const double* b_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_b_packed, - - // The unitless (packed) Jacobian, - // used by the internal optimization - // routines cholmod_analyze() and - // cholmod_factorize() require - // non-const - /* const */ - struct cholmod_sparse_struct* Jt, - - // meta-parameters - int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, - int Npoints, int Npoints_fixed, // at the end of points[] - int Nobservations_board, - int Nobservations_point, - - const mrcal_lensmodel_t* lensmodel, - mrcal_problem_selections_t problem_selections, - - int calibration_object_width_n, - int calibration_object_height_n); - //////////////////////////////////////////////////////////////////////////////// //////////////////// Layout of the measurement and state vectors @@ -722,45 +664,7 @@ int mrcal_measurement_index_points(int i_observation_point, int calibration_object_width_n, int calibration_object_height_n); int mrcal_num_measurements_points(int Nobservations_point); -int mrcal_measurement_index_points_triangulated(int i_point_triangulated, - int Nobservations_board, - int Nobservations_point, - - // May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - - int calibration_object_width_n, - int calibration_object_height_n); -int mrcal_num_measurements_points_triangulated(// May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated); -int mrcal_num_measurements_points_triangulated_initial_Npoints(// May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - - // Only consider the leading Npoints. If Npoints < 0: take ALL the points - int Npoints); -bool mrcal_decode_observation_indices_points_triangulated( - // output - int* iobservation0, - int* iobservation1, - int* iobservation_point0, - int* Nobservations_this_point, - int* Nmeasurements_this_point, - int* ipoint, - - // input - const int imeasurement, - - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated); - -int mrcal_measurement_index_regularization(// May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - - int calibration_object_width_n, +int mrcal_measurement_index_regularization(int calibration_object_width_n, int calibration_object_height_n, int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, @@ -775,11 +679,6 @@ int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_ int mrcal_num_measurements(int Nobservations_board, int Nobservations_point, - - // May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - int calibration_object_width_n, int calibration_object_height_n, int Ncameras_intrinsics, int Ncameras_extrinsics, diff --git a/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h b/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h index 7911f7e278e..c6c66587005 100644 --- a/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h +++ b/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h @@ -505,7 +505,7 @@ void mrcal_compose_r_full( // output int r_1_stride0 // in bytes. <= 0 means "contiguous" ); -// Special-case rotation compositions for the uncertainty computation +// Special-case rotation composition for the uncertainty computation // // Same as mrcal_compose_r() except // @@ -529,25 +529,3 @@ void mrcal_compose_r_tinyr0_gradientr0_full( // output const double* r_1, // (3,) array int r_1_stride0 // in bytes. <= 0 means "contiguous" ); -// Same as mrcal_compose_r() except -// -// - r1 is assumed to be 0, so we don't ingest it, and we don't report the -// composition result -// - we ONLY report the dr01/dr1 gradient -// -// In python this function is equivalent to -// -// _,_,dr01_dr1 = compose_r(r0, -// np.zeros((3,),), -// get_gradients=True) -#define mrcal_compose_r_tinyr1_gradientr1(dr_dr1,r_0) \ - mrcal_compose_r_tinyr1_gradientr1_full(dr_dr1,0,0,r_0,0) -void mrcal_compose_r_tinyr1_gradientr1_full( // output - double* dr_dr1, // (3,3) array; may be NULL - int dr_dr1_stride0, // in bytes. <= 0 means "contiguous" - int dr_dr1_stride1, // in bytes. <= 0 means "contiguous" - - // input - const double* r_0, // (3,) array - int r_0_stride0 // in bytes. <= 0 means "contiguous" - ); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/scales.h b/wpical/src/main/native/thirdparty/mrcal/include/scales.h deleted file mode 100644 index 14ed88ab382..00000000000 --- a/wpical/src/main/native/thirdparty/mrcal/include/scales.h +++ /dev/null @@ -1,43 +0,0 @@ -#pragma once - -// 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: -// -// b0,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 diff --git a/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h b/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h index cb685f95000..e3b623a31bd 100644 --- a/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h +++ b/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h @@ -129,25 +129,3 @@ mrcal_triangulate_leecivera_wmid2(// outputs // I don't implement triangulate_leecivera_l2() yet because it requires // computing an SVD, which is far slower than what the rest of these functions // do - -double -_mrcal_triangulated_error(// outputs - mrcal_point3_t* _derr_dv1, - mrcal_point3_t* _derr_dt01, - - // inputs - - // not-necessarily normalized vectors in the camera-0 - // coord system - const mrcal_point3_t* _v0, - const mrcal_point3_t* _v1, - const mrcal_point3_t* _t01); - -bool -_mrcal_triangulate_leecivera_mid2_is_convergent(// inputs - - // not-necessarily normalized vectors in the camera-0 - // coord system - const mrcal_point3_t* _v0, - const mrcal_point3_t* _v1, - const mrcal_point3_t* _t01); diff --git a/wpical/src/main/native/thirdparty/mrcal/src/mrcal.c b/wpical/src/main/native/thirdparty/mrcal/src/mrcal.c index 7fb77075fbc..6a211d82d2a 100644 --- a/wpical/src/main/native/thirdparty/mrcal/src/mrcal.c +++ b/wpical/src/main/native/thirdparty/mrcal/src/mrcal.c @@ -22,9 +22,49 @@ #include "mrcal.h" #include "minimath/minimath.h" #include "cahvore.h" -#include "minimath/minimath-extra.h" #include "util.h" -#include "scales.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: +// +// b0,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_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0) @@ -438,224 +478,21 @@ int mrcal_measurement_index_points(int i_observation_point, if(Nobservations_point <= 0) return -1; - // 2: x,y measurements + // 3: x,y measurements, range normalization return mrcal_num_measurements_boards(Nobservations_board, calibration_object_width_n, calibration_object_height_n) + - i_observation_point * 2; + i_observation_point * 3; } -#warning "triangulated-solve: need known-range points, at-infinity points" - int mrcal_num_measurements_points(int Nobservations_point) { - // 2: x,y measurements - return Nobservations_point * 2; -} - -#warning "triangulated-solve: Add a test for mrcal_measurement_index_points_triangulated()" -int mrcal_measurement_index_points_triangulated(int i_point_triangulated, - int Nobservations_board, - int Nobservations_point, - - // May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - - int calibration_object_width_n, - int calibration_object_height_n) -{ - if(observations_point_triangulated == NULL || - Nobservations_point_triangulated <= 0) - return -1; - - return - mrcal_num_measurements_boards(Nobservations_board, - calibration_object_width_n, - calibration_object_height_n) + - mrcal_num_measurements_points(Nobservations_point) + - mrcal_num_measurements_points_triangulated_initial_Npoints(observations_point_triangulated, - Nobservations_point_triangulated, - i_point_triangulated); -} - -#warning "triangulated-solve: python-wrap this function" -int mrcal_num_measurements_points_triangulated_initial_Npoints(// May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - - // Only consider the leading Npoints. If Npoints < 0: take ALL the points - int Npoints) -{ - if(observations_point_triangulated == NULL || - Nobservations_point_triangulated <= 0) - return 0; - - // Similar loop as in _mrcal_num_j_nonzero(). If the data layout changes, - // update this and that - - int Nmeas = 0; - int ipoint = 0; - int iobservation = 0; - - while( iobservation < Nobservations_point_triangulated && - (Npoints < 0 || ipoint < Npoints)) - { - int Nset = 1; - while(!observations_point_triangulated[iobservation].last_in_set) - { - Nmeas += Nset++; - iobservation++; - } - - ipoint++; - iobservation++; - } - - return Nmeas; -} - -int mrcal_num_measurements_points_triangulated(// May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated) -{ - return - mrcal_num_measurements_points_triangulated_initial_Npoints( observations_point_triangulated, - Nobservations_point_triangulated, - -1 ); -} - -#warning "triangulated-solve: python-wrap this function" -bool mrcal_decode_observation_indices_points_triangulated( - // output - int* iobservation0, - int* iobservation1, - int* iobservation_point0, - int* Nobservations_this_point, - int* Nmeasurements_this_point, - int* ipoint, - - // input - const int imeasurement, - - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated) -{ - if(observations_point_triangulated == NULL || - Nobservations_point_triangulated <= 0) - return false; - - // Similar loop as in - // mrcal_num_measurements_points_triangulated_initial_Npoints(). If the data - // layout changes, update this and that - - int Nmeas = 0; - int iobservation = 0; - *ipoint = 0; - - while( iobservation < Nobservations_point_triangulated ) - { - int Nset = 1; - while(!observations_point_triangulated[iobservation].last_in_set) - { - Nset++; - iobservation++; - } - - // This point has Nset observations. Each pair of observations produces - // a measurement, so: - const int Nmeas_thispoint = Nset*(Nset-1) / 2; - - // Done with this point. It is described by Nmeas_thispoint - // measurements, with the first one at Nmeas. The last observation is at - // iobservation - - if(imeasurement < Nmeas+Nmeas_thispoint) - { - // The measurement I care about observes THIS point. I find the - // specific observations. - // - // I simplify the notation: "m" is "imeasurement", "o" is - // "iobservation". - - const int No = Nset; - const int Nm = Nmeas_thispoint; - const int m = imeasurement - Nmeas; - - // Within this point o is in range(No) and m is in range(Nm). A pair - // (o0,o1) describes one measurement. Both o0 and o1 are in - // range(No) and o1>o0. A sample table of measurement indices m for - // No = 5: - // - // o1 - // 0 1 2 3 4 - // --------- - // 0| 0 1 2 3 - // o0 1| 4 5 6 - // 2| 7 8 - // 3| 9 - // 4| - // - // For a given o0, the maximum m = - // - // m_max = (No-1) + (No-2) + (No-3) ... - 1 - // - // for a total of o0+1 (No...) terms so - // - // m_max = ((No-1) + (No-o0-1))/2 * (o0+1) - 1 - // = (2*No - 2 - o0)/2 * (o0+1) - 1 - // - // m_min = m_max(o0-1) + 1 - // = (2*No - 2 - o0 + 1)/2 * o0 - // = (2*No - 1 - o0) * o0 / 2 - // = (-o0^2 + (2*No - 1)*o0 ) / 2 - // - // -> (-1/2) o0^2 + (2*No - 1)/2 o0 - m_min = 0 - // -> o0 = (2*No - 1)/2 +- sqrt( (2*No - 1)^2/4 - 2*m_min) - // = (2*No - 1)/2 - sqrt( (2*No - 1)^2/4 - 2*m_min) - // = (2*No - 1 - sqrt( (2*No - 1)^2 - 8*m_min))/2 - // - // o0(m) = floor(o0(m)) - // - // Now that I have o0 I compute - // - // o1 = m - m_min(o0) + o0 + 1 - // = m - (-o0^2 + (2*No - 1)*o0 ) / 2 + o0 + 1 - // = m + (o0^2 + (3- 2*No)*o0 + 2) / 2 - // = m + o0*(o0 + 3 - 2*No)/2 + 1 - const double x = 2.*(double)No - 1.; - *iobservation0 = (int)((x - sqrt( x*x - 8.*(double)m))/2.); - *iobservation1 = m + (*iobservation0)*((*iobservation0) + 3 - 2*No)/2 + 1; - - // Reference the observations from the first - *iobservation_point0 = iobservation-Nset+1; - *iobservation0 += *iobservation_point0; - *iobservation1 += *iobservation_point0; - - *Nobservations_this_point = No; - *Nmeasurements_this_point = Nm; - - return true; - } - - Nmeas += Nmeas_thispoint; - - iobservation++; - (*ipoint)++; - } - - return false; + // 3: x,y measurements, range normalization + return Nobservations_point * 3; } - -int mrcal_measurement_index_regularization( -#warning "triangulated-solve: this argument order is weird. Put then triangulated stuff at the end?" - // May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - - int calibration_object_width_n, +int mrcal_measurement_index_regularization(int calibration_object_width_n, int calibration_object_height_n, int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, @@ -675,9 +512,7 @@ int mrcal_measurement_index_regularization( mrcal_num_measurements_boards(Nobservations_board, calibration_object_width_n, calibration_object_height_n) + - mrcal_num_measurements_points(Nobservations_point) + - mrcal_num_measurements_points_triangulated(observations_point_triangulated, - Nobservations_point_triangulated); + mrcal_num_measurements_points(Nobservations_point); } int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_extrinsics, @@ -688,20 +523,11 @@ int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_ { return Ncameras_intrinsics * - num_regularization_terms_percamera(problem_selections, lensmodel) + - - ((problem_selections.do_apply_regularization_unity_cam01 && - problem_selections.do_optimize_extrinsics && - Ncameras_extrinsics > 0) ? 1 : 0); + num_regularization_terms_percamera(problem_selections, lensmodel); } int mrcal_num_measurements(int Nobservations_board, int Nobservations_point, - - // May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - int calibration_object_width_n, int calibration_object_height_n, int Ncameras_intrinsics, int Ncameras_extrinsics, @@ -715,8 +541,6 @@ int mrcal_num_measurements(int Nobservations_board, calibration_object_width_n, calibration_object_height_n) + mrcal_num_measurements_points(Nobservations_point) + - mrcal_num_measurements_points_triangulated(observations_point_triangulated, - Nobservations_point_triangulated) + mrcal_num_measurements_regularization(Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, Nobservations_board, @@ -732,11 +556,6 @@ static bool has_calobject_warp(mrcal_problem_selections_t problem_selections, int _mrcal_num_j_nonzero(int Nobservations_board, int Nobservations_point, - - // May be NULL if we don't have any of these - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - int calibration_object_width_n, int calibration_object_height_n, int Ncameras_intrinsics, int Ncameras_extrinsics, @@ -797,43 +616,16 @@ int _mrcal_num_j_nonzero(int Nobservations_board, if( problem_selections.do_optimize_extrinsics && observations_point[i].icam.extrinsics >= 0 ) N += 2*6; - } - - // And the triangulated point observations - if(observations_point_triangulated != NULL && - Nobservations_point_triangulated > 0) - { - // Similar loop as in - // mrcal_num_measurements_points_triangulated_initial_Npoints(). If the - // data layout changes, update this and that - for(int i0=0; i0= 0 ) - Nvars0 += 6; - - int i1 = i0; - do - { - i1++; - - int Nvars1 = Nintrinsics_per_measurement; - if( problem_selections.do_optimize_extrinsics && - observations_point_triangulated[i1].icam.extrinsics >= 0 ) - Nvars1 += 6; - - N += Nvars0 + Nvars1; - - } while(!observations_point_triangulated[i1].last_in_set); - } + // range normalization + if(problem_selections.do_optimize_frames && + observations_point[i].i_point < Npoints-Npoints_fixed ) + N += 3; + if( problem_selections.do_optimize_extrinsics && + observations_point[i].icam.extrinsics >= 0 ) + N += 6; } - // Regularization if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) { if(problem_selections.do_apply_regularization) @@ -858,14 +650,6 @@ int _mrcal_num_j_nonzero(int Nobservations_board, num_regularization_terms_percamera(problem_selections, lensmodel); - if(problem_selections.do_apply_regularization_unity_cam01 && - problem_selections.do_optimize_extrinsics && - Ncameras_extrinsics > 0) - { -#warning "triangulated-solve: I assume that camera0 is at the reference" - N += 3; // translation only - } - return N; } @@ -1516,6 +1300,9 @@ void mrcal_project_stereographic( // output // (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 // @@ -3936,7 +3723,7 @@ bool mrcal_corresponding_icam_extrinsics(// out if( !(Ncameras_intrinsics == Ncameras_extrinsics || Ncameras_intrinsics == Ncameras_extrinsics+1 ) ) { - MSG("Cannot compute icam_extrinsics. I don't have a vanilla calibration problem (stationary cameras, cam0 is reference)"); + MSG("Cannot compute icam_extrinsics. I don't have a vanilla calibration problem"); return false; } @@ -3965,11 +3752,6 @@ bool mrcal_corresponding_icam_extrinsics(// out // Doing this myself instead of hooking into the logic in libdogleg for now. // Bring back the fancy libdogleg logic once everything stabilizes -// -// Note: this does NOT process discrete points because big errors in these -// observations don't cause clear outliers in the measurement vector, from what -// I can see in test-sfm-fixed-points. If we can find a way to tie observation -// errors directly to measurements, please add that here static bool markOutliers(// output, input @@ -3978,26 +3760,15 @@ bool markOutliers(// output, input mrcal_point3_t* observations_board_pool, // output - int* Noutliers_board, - int* Noutliers_triangulated_point, + int* Noutliers, // input - - // for diagnostics only const mrcal_observation_board_t* observations_board, - int Nobservations_board, - int Nobservations_point, - -#warning "triangulated-solve: not const because this is where the outlier bit lives currently" - mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - int calibration_object_width_n, int calibration_object_height_n, const double* x_measurements, - const mrcal_pose_t* rt_extrinsics_fromref, // Ncameras_extrinsics of these bool verbose) { // I define an outlier as a feature that's > k stdevs past the mean. I make @@ -4005,7 +3776,7 @@ bool markOutliers(// output, input // with mean 0. This is reasonable because this function is applied after // running the optimization. // - // The threshold is based on the stdev of my observed residuals + // The threshold stdev is the stdev of my observed residuals // // 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 @@ -4014,307 +3785,99 @@ bool markOutliers(// output, input const double k0 = 4.0; const double k1 = 5.0; + *Noutliers = 0; - *Noutliers_board = 0; - int Ninliers_board = 0; - - *Noutliers_triangulated_point = 0; - int Ninliers_triangulated_point = 0; - - const int imeasurement_board0 = - mrcal_measurement_index_boards(0, - Nobservations_board, - Nobservations_point, - calibration_object_width_n, - calibration_object_height_n); - const int imeasurement_point_triangulated0 = - mrcal_measurement_index_points_triangulated(0, - Nobservations_board, - Nobservations_point, - - observations_point_triangulated, - Nobservations_point_triangulated, - - calibration_object_width_n, - calibration_object_height_n); + int i_pt,i_feature; - - // just in case - if(Nobservations_point_triangulated <= 0) - { - Nobservations_point_triangulated = 0; - observations_point_triangulated = NULL; - } - - const double* x_boards = - &x_measurements[ imeasurement_board0 ]; - const double* x_point_triangulated = - &x_measurements[ imeasurement_point_triangulated0 ]; - -#define LOOP_BOARD_OBSERVATION(extra_while_condition) \ - for(int i_observation_board=0, i_pt_board = 0; \ - i_observation_boardicam.intrinsics; \ - for(int i_pt=0; \ + for(i_pt=0; \ i_pt < calibration_object_width_n*calibration_object_height_n; \ - i_pt++, i_pt_board++) - -#define LOOP_BOARD_FEATURE_HEADER() \ - const mrcal_point3_t* pt_observed = &observations_board_pool[i_pt_board]; \ - double* weight = &observations_board_pool[i_pt_board].z; - -#define LOOP_TRIANGULATED_POINT0(extra_while_condition) \ - for(int i0 = 0, \ - imeasurement_point_triangulated = 0; \ - i0 < Nobservations_point_triangulated && extra_while_condition; \ - i0++) - -#define LOOP_TRIANGULATED_POINT1() \ - mrcal_observation_point_triangulated_t* pt0 = \ - &observations_point_triangulated[i0]; \ - if(pt0->last_in_set) \ - continue; \ - \ - int i1 = i0+1; - -#define LOOP_TRIANGULATED_POINT_HEADER() \ - mrcal_observation_point_triangulated_t* pt1 = \ - &observations_point_triangulated[i1]; - -#define LOOP_TRIANGULATED_POINT_FOOTER() \ - imeasurement_point_triangulated++; \ - if(pt1->last_in_set) \ - break; \ - i1++; + i_pt++, i_feature++) +#define LOOP_FEATURE_HEADER() \ + const mrcal_point3_t* pt_observed = &observations_board_pool[i_feature]; \ + double* weight = &observations_board_pool[i_feature].z; - /////////////// Compute the variance to set the threshold - bool foundNewOutliers = false; + int Ninliers = 0; double var = 0.0; - LOOP_BOARD_OBSERVATION(true) + + LOOP_OBSERVATION() { - LOOP_BOARD_FEATURE() + LOOP_FEATURE() { - LOOP_BOARD_FEATURE_HEADER(); + LOOP_FEATURE_HEADER(); if(*weight <= 0.0) { - (*Noutliers_board)++; + (*Noutliers)++; continue; } - double dx = x_boards[2*i_pt_board + 0]; - double dy = x_boards[2*i_pt_board + 1]; + double dx = x_measurements[2*i_feature + 0]; + double dy = x_measurements[2*i_feature + 1]; var += dx*dx + dy*dy; - Ninliers_board++; - } - } - - MSG("I started with %d board outliers", *Noutliers_board); - int Nmeasurements_outliers_triangulated_start = 0; - - LOOP_TRIANGULATED_POINT0(true) - { - LOOP_TRIANGULATED_POINT1(); - - const mrcal_point3_t* v0 = &pt0->px; - const mrcal_point3_t* t_r0; - mrcal_point3_t _t_r0; - const mrcal_point3_t* v0_ref; - mrcal_point3_t _v0_ref; - - const int icam_extrinsics0 = pt0->icam.extrinsics; - if( icam_extrinsics0 >= 0 ) - { - const mrcal_pose_t* rt_0r = &rt_extrinsics_fromref[icam_extrinsics0]; - const double* r_0r = &rt_0r->r.xyz[0]; - const double* t_0r = &rt_0r->t.xyz[0]; - - t_r0 = &_t_r0; - v0_ref = &_v0_ref; - - mrcal_rotate_point_r_inverted(_t_r0.xyz, NULL,NULL, - r_0r, t_0r); - for(int i=0; i<3; i++) - _t_r0.xyz[i] *= -1.; - - mrcal_rotate_point_r_inverted(_v0_ref.xyz, NULL,NULL, - r_0r, v0->xyz); - } - else - { - t_r0 = NULL; - v0_ref = v0; - } - - - while(true) - { - LOOP_TRIANGULATED_POINT_HEADER(); - - /////////////// divergent triangulated observations are DEFINITELY outliers - if(pt0->outlier || pt1->outlier) - Nmeasurements_outliers_triangulated_start++; - else - { - const mrcal_point3_t* v1 = &pt1->px; - - const mrcal_point3_t* t_10; - mrcal_point3_t _t_10; - const mrcal_point3_t* v0_cam1; - mrcal_point3_t _v0_cam1; - - const int icam_extrinsics1 = pt1->icam.extrinsics; - if( icam_extrinsics1 >= 0 ) - { - const mrcal_pose_t* rt_1r = &rt_extrinsics_fromref[icam_extrinsics1]; - - v0_cam1 = &_v0_cam1; - - if( icam_extrinsics0 >= 0 ) - { - t_10 = &_t_10; - mrcal_transform_point_rt( &_t_10.xyz[0], NULL, NULL, - &rt_1r->r.xyz[0], &t_r0->xyz[0] ); - } - else - { - // t_r0 = 0 -> - // - // t_10 = R_1r*t_r0 + t_1r = - // = R_1r*0 + t_1r = - // = t_1r - t_10 = &rt_1r->t; - } - - mrcal_rotate_point_r( &_v0_cam1.xyz[0], NULL, NULL, - &rt_1r->r.xyz[0], &v0_ref->xyz[0] ); - } - else - { - // rt_1r = 0 -> - // - // t_10 = R_1r*t_r0 + t_1r = - // = t_r0 - t_10 = t_r0; - // At most one camera can sit at the reference. So if I'm - // here, I know that t_r0 != NULL and thus t_10 != NULL - - v0_cam1 = v0_ref; - } - if(!_mrcal_triangulate_leecivera_mid2_is_convergent(v1, v0_cam1, t_10)) - { - // Outlier. I don't know which observations was the broken - // one, so I mark them both - pt0->outlier = true; - pt1->outlier = true; - foundNewOutliers = true; -#warning "triangulated-solve: outliers should not be marked in this first loop. This should happen in the following loop. Putting it here breaks the logic" - - - // There are a lot of these, so I'm disabling this print for - // now, to avoid spamming the terminal - // - // MSG("New divergent-feature outliers found: measurement %d observation (%d,%d)", - // imeasurement_point_triangulated0 + imeasurement_point_triangulated, - // i0, i1); - - } - } - // just marked divergent triangulations as outliers - - - if(pt0->outlier || pt1->outlier) - (*Noutliers_triangulated_point)++; - else - { - var += - x_point_triangulated[imeasurement_point_triangulated] * - x_point_triangulated[imeasurement_point_triangulated]; - Ninliers_triangulated_point++; - } - LOOP_TRIANGULATED_POINT_FOOTER(); + Ninliers++; } } - if(Nobservations_point_triangulated > 0) - { - MSG("I started with %d triangulated outliers", Nmeasurements_outliers_triangulated_start); - MSG("I started with %d triangulated outliers", *Noutliers_triangulated_point); - } - var /= (double)(Ninliers_board*2 + Ninliers_triangulated_point); - // MSG("Outlier rejection sees stdev = %f", sqrt(var)); + var /= (double)(2*Ninliers); - ///////////// Any new outliers found? - LOOP_BOARD_OBSERVATION(!foundNewOutliers) + bool markedAny = false; + LOOP_OBSERVATION() { - LOOP_BOARD_FEATURE() + LOOP_FEATURE() { - LOOP_BOARD_FEATURE_HEADER(); + LOOP_FEATURE_HEADER(); - if(*weight <= 0.0) - continue; - - double dx = x_boards[2*i_pt_board + 0]; - double dy = x_boards[2*i_pt_board + 1]; - // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma - // -> x^2 > k^2 var - if(dx*dx > k1*k1*var || - dy*dy > k1*k1*var ) - { - // MSG("Feature %d looks like an outlier. x/y are %f/%f stdevs off mean (assumed 0). Observed stdev: %f, limit: %f", - // i_pt_board, - // dx/sqrt(var), - // dy/sqrt(var), - // sqrt(var), - // k1); - foundNewOutliers = true; - break; - } - } - } - LOOP_TRIANGULATED_POINT0(!foundNewOutliers) - { - LOOP_TRIANGULATED_POINT1(); - while(true) - { - LOOP_TRIANGULATED_POINT_HEADER(); - - if(!pt0->outlier && !pt1->outlier) { - double dx = x_point_triangulated[imeasurement_point_triangulated]; + if(*weight <= 0.0) + continue; + double dx = x_measurements[2*i_feature + 0]; + double dy = x_measurements[2*i_feature + 1]; // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma // -> x^2 > k^2 var - if(dx*dx > k1*k1*var ) + if(dx*dx > k1*k1*var || + dy*dy > k1*k1*var ) { - foundNewOutliers = true; - break; + *weight *= -1.0; + markedAny = true; + (*Noutliers)++; + // MSG("Feature %d looks like an outlier. x/y are %f/%f stdevs off mean (assumed 0). Observed stdev: %f, limit: %f", + // i_feature, + // dx/sqrt(var), + // dy/sqrt(var), + // sqrt(var), + // k1); } } - LOOP_TRIANGULATED_POINT_FOOTER(); } } - if(!foundNewOutliers) + + if(!markedAny) return false; - // I have new outliers: some measurements were found past the threshold. I - // throw out a bit extra to leave some margin so that the next - // re-optimization would hopefully be the last. - LOOP_BOARD_OBSERVATION(true) + // 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_OBSERVATION() { int Npt_inlier = 0; int Npt_outlier = 0; - LOOP_BOARD_FEATURE() + LOOP_FEATURE() { - LOOP_BOARD_FEATURE_HEADER(); + LOOP_FEATURE_HEADER(); if(*weight <= 0.0) { Npt_outlier++; @@ -4322,15 +3885,15 @@ bool markOutliers(// output, input } Npt_inlier++; - double dx = x_boards[2*i_pt_board + 0]; - double dy = x_boards[2*i_pt_board + 1]; + double dx = x_measurements[2*i_feature + 0]; + double dy = x_measurements[2*i_feature + 1]; // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma // -> x^2 > k^2 var if(dx*dx > k0*k0*var || dy*dy > k0*k0*var ) { *weight *= -1.0; - (*Noutliers_board)++; + (*Noutliers)++; } } @@ -4343,51 +3906,14 @@ bool markOutliers(// output, input Npt_inlier, Npt_inlier + Npt_outlier); } - LOOP_TRIANGULATED_POINT0(true) - { - LOOP_TRIANGULATED_POINT1(); - while(true) - { - LOOP_TRIANGULATED_POINT_HEADER(); - - if(!pt0->outlier && !pt1->outlier) - { - double dx = x_point_triangulated[imeasurement_point_triangulated]; - - // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma - // -> x^2 > k^2 var - if(dx*dx > k0*k0*var ) - { - // Outlier. I don't know which observations was the broken - // one, so I mark them both - pt0->outlier = true; - pt1->outlier = true; - - (*Noutliers_triangulated_point)++; - -#warning "triangulated-solve: outliers not returned to the caller yet, so I simply print them out here" - MSG("New outliers found: measurement %d observation (%d,%d)", - imeasurement_point_triangulated0 + imeasurement_point_triangulated, - i0, i1); - } - } - - LOOP_TRIANGULATED_POINT_FOOTER(); - } - } return true; -#undef LOOP_BOARD_OBSERVATION -#undef LOOP_BOARD_FEATURE -#undef LOOP_BOARD_FEATURE_HEADER -#undef LOOP_TRIANGULATED_POINT0 -#undef LOOP_TRIANGULATED_POINT1 -#undef LOOP_TRIANGULATED_POINT_HEADER -#undef LOOP_TRIANGULATED_POINT_FOOTER +#undef LOOP_OBSERVATION +#undef LOOP_FEATURE +#undef LOOP_FEATURE_HEADER } - typedef struct { // these are all UNPACKED @@ -4406,19 +3932,15 @@ typedef struct int Nobservations_board; const mrcal_observation_point_t* observations_point; - const mrcal_point3_t* observations_point_pool; int Nobservations_point; - const mrcal_observation_point_triangulated_t* observations_point_triangulated; - int Nobservations_point_triangulated; - bool verbose; mrcal_lensmodel_t lensmodel; mrcal_projection_precomputed_t precomputed; const int* imagersizes; // Ncameras_intrinsics*2 of these - mrcal_problem_selections_t problem_selections; + mrcal_problem_selections_t problem_selections; const mrcal_problem_constants_t* problem_constants; double calibration_object_spacing; @@ -4426,6 +3948,7 @@ typedef struct int calibration_object_height_n; const int Nmeasurements, N_j_nonzero, Nintrinsics; + const char* reportFitMsg; } callback_context_t; static @@ -4522,19 +4045,6 @@ void optimizer_callback(// input state } \ iJacobian += N; \ } while(0) -#define SCALE_JACOBIAN_N(col0, scale, N) \ - do \ - { \ - if(Jt) { \ - for(int i=0; ilensmodel) ? 4 : 0; @@ -4729,9 +4239,13 @@ void optimizer_callback(// input state { const double err = (q_hypothesis[i_pt].xy[i_xy] - qx_qy_w__observed->xyz[i_xy]) * weight; - if(ctx->verbose) - MSG("obs/frame/cam_i/cam_e/dot: %d %d %d %d %d err: %g", + 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; @@ -4851,9 +4365,13 @@ void optimizer_callback(// input state { const double err = 0.0; - if(ctx->verbose) - MSG( "obs/frame/cam_i/cam_e/dot: %d %d %d %d %d err: %g", + 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; @@ -4930,7 +4448,7 @@ void optimizer_callback(// input state ctx->problem_selections.do_optimize_frames && i_point < ctx->Npoints - ctx->Npoints_fixed; - const mrcal_point3_t* qx_qy_w__observed = &ctx->observations_point_pool[i_observation_point]; + const mrcal_point3_t* qx_qy_w__observed = &observation->px; double weight = qx_qy_w__observed->z; if(weight <= 0.0) @@ -5005,9 +4523,6 @@ void optimizer_callback(// input state iMeasurement++; } - // TEMPORARY TWEAK: disable range normalization - // I will re-add this later -#if 0 if(Jt) Jrowptr[iMeasurement] = iJacobian; x[iMeasurement] = 0; if(icam_extrinsics >= 0 && ctx->problem_selections.do_optimize_extrinsics ) @@ -5018,7 +4533,6 @@ void optimizer_callback(// input state if( use_position_from_state ) STORE_JACOBIAN3( i_var_point, 0,0,0 ); iMeasurement++; -#endif continue; } @@ -5192,10 +4706,6 @@ void optimizer_callback(// input state iMeasurement++; } - - // TEMPORARY TWEAK: disable range normalization - // I will re-add this later -#if 0 // Now the range normalization (make sure the range isn't // aphysically high or aphysically low) if(icam_extrinsics < 0) @@ -5306,531 +4816,49 @@ void optimizer_callback(// input state 2.0*dpenalty_ddistsq*(pcam.x*Rc[2] + pcam.y*Rc[5] + pcam.z*Rc[8]) ); iMeasurement++; } -#endif } - // Handle all the triangulated point observations. This is VERY similar to - // the board-observation loop above. Please consolidate - if( ctx->observations_point_triangulated != NULL && - ctx->Nobservations_point_triangulated ) + + ///////////////// Regularization + if(ctx->problem_selections.do_apply_regularization && + (ctx->problem_selections.do_optimize_intrinsics_distortions || + ctx->problem_selections.do_optimize_intrinsics_core)) { - if( ! (!ctx->problem_selections.do_optimize_intrinsics_core && - !ctx->problem_selections.do_optimize_intrinsics_distortions && - ctx->problem_selections.do_optimize_extrinsics ) ) - { - // Shouldn't get here. Have a check with an error message in - // mrcal_optimize() and mrcal_optimizer_callback() - assert(0); - } + const bool dump_regularizaton_details = false; -#warning "triangulated-solve: mrcal_observation_point_t.px is the observation vector in camera coords. No outliers. No intrinsics" -#warning "triangulated-solve: make weights work somehow. This is tied to outliers" + // 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: + // + // Nregularization_types = 2; + // Nmeasurements_rest*normal_pixel_error_sq * 0.005/Nregularization_types = + // Nmeasurements_regularization_distortion *normal_regularization_distortion_error_sq = + // Nmeasurements_regularization_centerpixel*normal_regularization_centerpixel_error_sq = + // + // normal_regularization_distortion_error_sq = (scale*normal_distortion_offset )^2 + // normal_regularization_centerpixel_error_sq = (scale*normal_centerpixel_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 int Nregularization_types = 2; - for(int i0 = 0; - i0 < ctx->Nobservations_point_triangulated; - i0++) - { - const mrcal_observation_point_triangulated_t* pt0 = - &ctx->observations_point_triangulated[i0]; - if(pt0->last_in_set) - continue; + int Nmeasurements_regularization_distortion = 0; + if(ctx->problem_selections.do_optimize_intrinsics_distortions) + Nmeasurements_regularization_distortion = + ctx->Ncameras_intrinsics*(ctx->Nintrinsics-Ncore); - if(!pt0->outlier) - { - // For better code efficiency I compute the triangulation in the - // camera-1 coord system. This is because this code looks like - // - // for(point0) - // { - // compute_stuff_for_point0; - // for(point1) - // { - // compute_stuff_for_point1; - // compute stuff based on products of point0 and point1; - // } - // } - // - // Doing the triangulation in the frame of point1 allows me to do - // more stuff in the outer compute_stuff_for_point0 computation, and - // less in the inner compute_stuff_for_point1 computation - - // I need t10. I'm looking at a composition Rt_10 = Rt_1r*Rt_r0 = - // (R_1r,t_1r)*(R_r0,t_r0) = (R_10, R_1r*t_r0 + t_1r) -> t_10 = - // R_1r*t_r0 + t_1r = transform(Rt_1r, t_r0) - // - // I don't actually have t_r0: I have t_0r, so I need to compute an - // inversion. y = R x + t -> x = Rinv y - Rinv t -> tinv = -Rinv t - // t_r0 = -R_r0 t_0r - - const mrcal_point3_t* v0 = &pt0->px; - - const mrcal_point3_t* t_r0; - mrcal_point3_t _t_r0; - double dnegt_r0__dr_0r[3][3]; - double dnegt_r0__dt_0r[3][3]; - - const mrcal_point3_t* v0_ref; - mrcal_point3_t _v0_ref; - double dv0_ref__dr_0r[3][3]; - - const int icam_extrinsics0 = pt0->icam.extrinsics; - if( icam_extrinsics0 >= 0 ) - { - const mrcal_pose_t* rt_0r = &camera_rt[icam_extrinsics0]; - const double* r_0r = &rt_0r->r.xyz[0]; - const double* t_0r = &rt_0r->t.xyz[0]; - - t_r0 = &_t_r0; - v0_ref = &_v0_ref; - - mrcal_rotate_point_r_inverted(_t_r0.xyz, - &dnegt_r0__dr_0r[0][0], - &dnegt_r0__dt_0r[0][0], - - r_0r, t_0r); - for(int i=0; i<3; i++) - _t_r0.xyz[i] *= -1.; - - mrcal_rotate_point_r_inverted(_v0_ref.xyz, - &dv0_ref__dr_0r[0][0], - NULL, - - r_0r, v0->xyz); - } - else - { - t_r0 = NULL; - v0_ref = v0; - } - - - int i1 = i0+1; - - while(true) - { - const mrcal_observation_point_triangulated_t* pt1 = - &ctx->observations_point_triangulated[i1]; - - if(!pt1->outlier) - { - const mrcal_point3_t* v1 = &pt1->px; - - const mrcal_point3_t* t_10; - mrcal_point3_t _t_10; - const mrcal_point3_t* v0_cam1; - mrcal_point3_t _v0_cam1; - - double dt_10__drt_1r [3][6]; - double dt_10__dt_r0 [3][3]; - double dv0_cam1__dr_1r [3][3]; - double dv0_cam1__dv0_ref[3][3]; - - const int icam_extrinsics1 = pt1->icam.extrinsics; - if( icam_extrinsics1 >= 0 ) - { - const mrcal_pose_t* rt_1r = &camera_rt[icam_extrinsics1]; - - v0_cam1 = &_v0_cam1; - - - if( icam_extrinsics0 >= 0 ) - { - t_10 = &_t_10; - mrcal_transform_point_rt( &_t_10.xyz[0], - &dt_10__drt_1r[0][0], - &dt_10__dt_r0 [0][0], - &rt_1r->r.xyz[0], &t_r0->xyz[0] ); - } - else - { - // t_r0 = 0 -> - // - // t_10 = R_1r*t_r0 + t_1r = - // = R_1r*0 + t_1r = - // = t_1r - t_10 = &rt_1r->t; - } - - mrcal_rotate_point_r( &_v0_cam1.xyz[0], - &dv0_cam1__dr_1r [0][0], - &dv0_cam1__dv0_ref[0][0], - &rt_1r->r.xyz[0], &v0_ref->xyz[0] ); - } - else - { - // rt_1r = 0 -> - // - // t_10 = R_1r*t_r0 + t_1r = - // = t_r0 - t_10 = t_r0; - // At most one camera can sit at the reference. So if I'm - // here, I know that t_r0 != NULL and thus t_10 != NULL - - v0_cam1 = v0_ref; - } - - mrcal_point3_t derr__dv0_cam1; - mrcal_point3_t derr__dt_10; - - double err = - _mrcal_triangulated_error(&derr__dv0_cam1, &derr__dt_10, - v1, v0_cam1, t_10); - - - x[iMeasurement] = err; - norm2_error += err*err; - - if(Jt) - { - Jrowptr[iMeasurement] = iJacobian; - - // Now I propagate gradients. Dependency graph: - // - // derr__dv0_cam1 - // dv0_cam1__dr_1r - // dv0_cam1__dv0_ref - // dv0_ref__dr_0r - // - // derr__dt_10 - // dt_10__drt_1r - // dt_10__dt_r0 - // dnegt_r0__dr_0r - // dnegt_r0__dt_0r - // - // So - // - // derr/dr_0r = - // derr/dv0_cam1 dv0_cam1/dv0_ref dv0_ref/dr_0r + - // derr/dt_10 dt_10/dt_r0 dt_r0/dr_0r - // - // derr/dt_0r = - // derr/dt_10 dt_10/dt_r0 dt_r0/dt_0r - // - // derr/dr_1r = - // derr/dv0_cam1 dv0_cam1/dr_1r + - // derr/dt_10 dt_10/dr_1r - // - // derr/dt_1r = - // derr/dt_10 dt_10/dt_1r - if( icam_extrinsics0 >= 0 ) - { - const int i_var_camera_rt0 = - mrcal_state_index_extrinsics(icam_extrinsics0, - ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, - ctx->Nframes, - ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, - ctx->problem_selections, &ctx->lensmodel); - - double* out; - - out = &Jval[iJacobian]; - double* derr__dt_r0; - double _derr__dt_r0[3]; - - if( icam_extrinsics1 >= 0 ) - { - derr__dt_r0 = _derr__dt_r0; - mul_vec3t_gen33(derr__dt_r0, derr__dt_10.xyz, &dt_10__dt_r0[0][0], 1, ); - - double temp[3]; - mul_vec3t_gen33(temp, derr__dv0_cam1.xyz, &dv0_cam1__dv0_ref[0][0], 1, ); - mul_vec3t_gen33(out, temp, &dv0_ref__dr_0r[0][0], 1, ); - } - else - { - // camera1 is at the reference, so I don't have - // dt_10__dt_r0 and dv0_cam1__dv0_ref explicitly - // stored. - // - // t_10 = t_r0 --> dt_10__dt_r0 = I - // v0_cam1 = v0_ref --> dv0_cam1__dv0_ref = I - derr__dt_r0 = derr__dt_10.xyz; - mul_vec3t_gen33(out, derr__dv0_cam1.xyz, &dv0_ref__dr_0r[0][0], 1, ); - } - - - mul_vec3t_gen33(out, derr__dt_r0, &dnegt_r0__dr_0r[0][0], -1, _accum); - - SCALE_JACOBIAN_N( i_var_camera_rt0 + 0, - SCALE_ROTATION_CAMERA, - 3 ); - - - out = &Jval[iJacobian]; - mul_vec3t_gen33(out, derr__dt_r0, &dnegt_r0__dt_0r[0][0], -1, ); - - SCALE_JACOBIAN_N( i_var_camera_rt0 + 3, - SCALE_TRANSLATION_CAMERA, - 3 ); - } - if( icam_extrinsics1 >= 0 ) - { - const int i_var_camera_rt1 = - mrcal_state_index_extrinsics(icam_extrinsics1, - ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, - ctx->Nframes, - ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, - ctx->problem_selections, &ctx->lensmodel); - - double* out; - - out = &Jval[iJacobian]; - - // derr/dr_1r = - // derr/dv0_cam1 dv0_cam1/dr_1r + - // derr/dt_10 dt_10/dr_1r - // - // derr/dt_1r = - // derr/dt_10 dt_10/dt_1r - mul_vec3t_gen33(out, - derr__dv0_cam1.xyz, - &dv0_cam1__dr_1r[0][0], - 1, - ); - - if( icam_extrinsics0 >= 0 ) - { - mul_genNM_genML_accum(out, 3,1, - 1,3,3, - derr__dt_10.xyz, 3,1, - &dt_10__drt_1r[0][0], 6, 1, - 1); - SCALE_JACOBIAN_N( i_var_camera_rt1 + 0, - SCALE_ROTATION_CAMERA, - 3 ); - - out = &Jval[iJacobian]; - mul_genNM_genML(out, 3,1, - 1,3,3, - derr__dt_10.xyz, 3,1, - &dt_10__drt_1r[0][3], 6, 1, - 1); - - SCALE_JACOBIAN_N( i_var_camera_rt1 + 3, - SCALE_TRANSLATION_CAMERA, - 3 ); - } - else - { - // camera0 is at the reference. dt_10__drt_1r is not - // given explicitly - // - // t_10 = t_1r -> - // dt_10__dr_1r = 0 - // dt_10__dt_1r = I - // So - // - // derr/dr_1r = derr/dv0_cam1 dv0_cam1/dr_1r - // derr/dt_1r = derr/dt_10 - SCALE_JACOBIAN_N( i_var_camera_rt1 + 0, - SCALE_ROTATION_CAMERA, - 3 ); - - out = &Jval[iJacobian]; - - for(int i=0; i<3; i++) - out[i] = derr__dt_10.xyz[i]; - - SCALE_JACOBIAN_N( i_var_camera_rt1 + 3, - SCALE_TRANSLATION_CAMERA, - 3 ); - } - } - } - else - { - // Don't need the Jacobian. I just move iJacobian as needed - if( icam_extrinsics0 >= 0 ) - iJacobian += 6; - if( icam_extrinsics1 >= 0 ) - iJacobian += 6; - } - } - else - { - // pt1 is an outlier - const double err = 0.0; - - const int icam_extrinsics1 = pt1->icam.extrinsics; - - x[iMeasurement] = err; - norm2_error += err*err; - - if(Jt) - { - Jrowptr[iMeasurement] = iJacobian; - - if( icam_extrinsics0 >= 0 ) - { - const int i_var_camera_rt0 = - mrcal_state_index_extrinsics(icam_extrinsics0, - ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, - ctx->Nframes, - ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, - ctx->problem_selections, &ctx->lensmodel); - - STORE_JACOBIAN_N( i_var_camera_rt0 + 0, - (double*)NULL, 0.0, - 3 ); - STORE_JACOBIAN_N( i_var_camera_rt0 + 3, - (double*)NULL, 0.0, - 3 ); - } - if( icam_extrinsics1 >= 0 ) - { - const int i_var_camera_rt1 = - mrcal_state_index_extrinsics(icam_extrinsics1, - ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, - ctx->Nframes, - ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, - ctx->problem_selections, &ctx->lensmodel); - STORE_JACOBIAN_N( i_var_camera_rt1 + 0, - (double*)NULL, 0.0, - 3 ); - STORE_JACOBIAN_N( i_var_camera_rt1 + 3, - (double*)NULL, 0.0, - 3 ); - } - } - else - { - // Don't need the Jacobian. I just move iJacobian as needed - if( icam_extrinsics0 >= 0 ) - iJacobian += 6; - if( icam_extrinsics1 >= 0 ) - iJacobian += 6; - } - } - - iMeasurement++; - - if(pt1->last_in_set) - break; - i1++; - } - } - else - { - // pt0 is an outlier. I loop through all the pairwise - // observations, but I ignore ALL of them - const double err = 0.0; - - const int icam_extrinsics0 = pt0->icam.extrinsics; - int i1 = i0+1; - - while(true) - { - const mrcal_observation_point_triangulated_t* pt1 = - &ctx->observations_point_triangulated[i1]; - - const int icam_extrinsics1 = pt1->icam.extrinsics; - - x[iMeasurement] = err; - norm2_error += err*err; - - if(Jt) - { - Jrowptr[iMeasurement] = iJacobian; - - if( icam_extrinsics0 >= 0 ) - { - const int i_var_camera_rt0 = - mrcal_state_index_extrinsics(icam_extrinsics0, - ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, - ctx->Nframes, - ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, - ctx->problem_selections, &ctx->lensmodel); - - STORE_JACOBIAN_N( i_var_camera_rt0 + 0, - (double*)NULL, 0.0, - 3 ); - STORE_JACOBIAN_N( i_var_camera_rt0 + 3, - (double*)NULL, 0.0, - 3 ); - } - if( icam_extrinsics1 >= 0 ) - { - const int i_var_camera_rt1 = - mrcal_state_index_extrinsics(icam_extrinsics1, - ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, - ctx->Nframes, - ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, - ctx->problem_selections, &ctx->lensmodel); - - STORE_JACOBIAN_N( i_var_camera_rt1 + 0, - (double*)NULL, 0.0, - 3 ); - STORE_JACOBIAN_N( i_var_camera_rt1 + 3, - (double*)NULL, 0.0, - 3 ); - } - } - else - { - // Don't need the Jacobian. I just move iJacobian as needed - if( icam_extrinsics0 >= 0 ) - iJacobian += 6; - if( icam_extrinsics1 >= 0 ) - iJacobian += 6; - } - - iMeasurement++; - - if(pt1->last_in_set) - break; - i1++; - } - } - } - } - - ///////////////// Regularization - { - const bool dump_regularizaton_details = false; - - // 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/Nregularization_types = - // Nmeasurements_regularization_distortion *normal_regularization_distortion_error_sq = - // Nmeasurements_regularization_centerpixel*normal_regularization_centerpixel_error_sq = - // Nmeasurements_regularization_unity_cam01*normal_regularization_unity_cam01_error_sq - // - // normal_regularization_distortion_error_sq = (scale*normal_distortion_offset )^2 - // normal_regularization_centerpixel_error_sq = (scale*normal_centerpixel_value )^2 - // normal_regularization_unity_cam01_error_sq = (scale*normal_unity_cam01_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. - int Nmeasurements_regularization_distortion = 0; int Nmeasurements_regularization_centerpixel = 0; - int Nmeasurements_regularization_unity_cam01 = 0; - - if(ctx->problem_selections.do_apply_regularization) - { - if(ctx->problem_selections.do_optimize_intrinsics_distortions) - Nmeasurements_regularization_distortion = - ctx->Ncameras_intrinsics*(ctx->Nintrinsics-Ncore); - if(ctx->problem_selections.do_optimize_intrinsics_core) - Nmeasurements_regularization_centerpixel = - ctx->Ncameras_intrinsics*2; - } - if(ctx->problem_selections.do_apply_regularization_unity_cam01 && - ctx->problem_selections.do_optimize_extrinsics && - ctx->Ncameras_extrinsics > 0) - { - Nmeasurements_regularization_unity_cam01 = 1; - } + if(ctx->problem_selections.do_optimize_intrinsics_core) + Nmeasurements_regularization_centerpixel = + ctx->Ncameras_intrinsics*2; int Nmeasurements_nonregularization = ctx->Nmeasurements - (Nmeasurements_regularization_distortion + - Nmeasurements_regularization_centerpixel + - Nmeasurements_regularization_unity_cam01); + Nmeasurements_regularization_centerpixel); double normal_pixel_error = 1.0; double expected_total_pixel_error_sq = @@ -5840,311 +4868,249 @@ void optimizer_callback(// input state if(dump_regularizaton_details) MSG("expected_total_pixel_error_sq: %f", expected_total_pixel_error_sq); - // This is set to 2 to match what mrcal 2.4 does, to keep the behavior - // consistent. The exact value doesn't matter. In a previous commit (the - // merge 5c3bdd2b) this was changed to 3, and I'm about to revert it - // back to 2 (2024/07) - const int Nregularization_types = 2; + double scale_regularization_distortion = 0.0; + double scale_regularization_centerpixel = 0.0; - if(ctx->problem_selections.do_apply_regularization && - (ctx->problem_selections.do_optimize_intrinsics_distortions || - ctx->problem_selections.do_optimize_intrinsics_core)) + // compute scales { - double scale_regularization_distortion = 0.0; - double scale_regularization_centerpixel = 0.0; - - // compute scales + if(ctx->problem_selections.do_optimize_intrinsics_distortions) { - if(ctx->problem_selections.do_optimize_intrinsics_distortions) - { - // I need to control this better, but this is sufficient for - // now. I need 2.0e-1 for splined models to effectively - // eliminate the curl in the splined model vector field. For - // other models I use 2.0 because that's what I had for a long - // time, and I don't want to change it to not break anything - double normal_distortion_value = - ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC ? - 2.0e-1 : - 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/(double)Nregularization_types / expected_regularization_distortion_error_sq_noscale; + // I need to control this better, but this is sufficient for + // now. I need 2.0e-1 for splined models to effectively + // eliminate the curl in the splined model vector field. For + // other models I use 2.0 because that's what I had for a long + // time, and I don't want to change it to not break anything + double normal_distortion_value = + ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC ? + 2.0e-1 : + 2.0; + + double expected_regularization_distortion_error_sq_noscale = + (double)Nmeasurements_regularization_distortion * + normal_distortion_value * + normal_distortion_value; - if(dump_regularizaton_details) - MSG("expected_regularization_distortion_error_sq: %f", expected_regularization_distortion_error_sq_noscale*scale_sq); + double scale_sq = + expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_distortion_error_sq_noscale; - scale_regularization_distortion = sqrt(scale_sq); - } + if(dump_regularizaton_details) + MSG("expected_regularization_distortion_error_sq: %f", expected_regularization_distortion_error_sq_noscale*scale_sq); - if(modelHasCore_fxfycxcy(&ctx->lensmodel) && - ctx->problem_selections.do_optimize_intrinsics_core) - { - double normal_centerpixel_offset = 500.0; + scale_regularization_distortion = sqrt(scale_sq); + } - double expected_regularization_centerpixel_error_sq_noscale = - (double)Nmeasurements_regularization_centerpixel * - normal_centerpixel_offset * - normal_centerpixel_offset; + if(modelHasCore_fxfycxcy(&ctx->lensmodel) && + ctx->problem_selections.do_optimize_intrinsics_core) + { + double normal_centerpixel_offset = 500.0; - double scale_sq = - expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_centerpixel_error_sq_noscale; + double expected_regularization_centerpixel_error_sq_noscale = + (double)Nmeasurements_regularization_centerpixel * + normal_centerpixel_offset * + normal_centerpixel_offset; - if(dump_regularizaton_details) - MSG("expected_regularization_centerpixel_error_sq: %f", expected_regularization_centerpixel_error_sq_noscale*scale_sq); + double scale_sq = + expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_centerpixel_error_sq_noscale; - scale_regularization_centerpixel = sqrt(scale_sq); - } + if(dump_regularizaton_details) + MSG("expected_regularization_centerpixel_error_sq: %f", expected_regularization_centerpixel_error_sq_noscale*scale_sq); + + scale_regularization_centerpixel = sqrt(scale_sq); } + } - // compute and store regularization terms - { - if( ctx->problem_selections.do_optimize_intrinsics_distortions ) - for(int icam_intrinsics=0; icam_intrinsicsNcameras_intrinsics; icam_intrinsics++) + // compute and store regularization terms + { + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + for(int icam_intrinsics=0; icam_intrinsicsNcameras_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_selections, &ctx->lensmodel); + + if(ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) { - 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_selections, &ctx->lensmodel); - - if(ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) - { - // Splined model regularization. I do directional L2 - // regularization. At each knot I penalize contributions in - // the tangential direction much more than in the radial - // direction. Otherwise noise in the data produces lots of - // curl in the vector field. This isn't wrong, but it's much - // nicer if "right" in the camera coordinate system - // corresponds to "right" in pixel space - const int Nx = ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx; - const int Ny = ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny; - - for(int iy=0; iyNintrinsics-Ncore; j++) + // Splined model regularization. I do directional L2 + // regularization. At each knot I penalize contributions in + // the tangential direction much more than in the radial + // direction. Otherwise noise in the data produces lots of + // curl in the vector field. This isn't wrong, but it's much + // nicer if "right" in the camera coordinate system + // corresponds to "right" in pixel space + const int Nx = ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx; + const int Ny = ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny; + + for(int iy=0; iylensmodel.type) && - ctx->lensmodel.type >= MRCAL_LENSMODEL_OPENCV8 && - 5 <= j && j <= 7 ) + const int ivar = 2*( iy*Nx + ix ); + const double deltauxy[] = + { intrinsics_all[icam_intrinsics][Ncore + ivar + 0], + intrinsics_all[icam_intrinsics][Ncore + ivar + 1] }; + + // WARNING: "Precompute uxy. This is lots of unnecessary computation in the inner loop" + double uxy[] = { (double)(2*ix - Nx + 1), + (double)(2*iy - Ny + 1) }; + bool anisotropic = true; + if(2*ix == Nx - 1 && + 2*iy == Ny - 1 ) + { + uxy[0] = 1.0; + anisotropic = false; + } + else { - // 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 mag = sqrt(uxy[0]*uxy[0] + uxy[1]*uxy[1]); + uxy[0] /= mag; + uxy[1] /= mag; } + double err; + + // I penalize radial corrections if(Jt) Jrowptr[iMeasurement] = iJacobian; - double err = scale*intrinsics_all[icam_intrinsics][j+Ncore]; + err = scale*(deltauxy[0]*uxy[0] + + deltauxy[1]*uxy[1]); x[iMeasurement] = err; norm2_error += err*err; - - STORE_JACOBIAN( i_var_intrinsics + Ncore_state + j, - scale * SCALE_DISTORTION ); - + STORE_JACOBIAN( i_var_intrinsics + Ncore_state + ivar + 0, + scale * uxy[0] * SCALE_DISTORTION ); + STORE_JACOBIAN( i_var_intrinsics + Ncore_state + ivar + 1, + scale * uxy[1] * SCALE_DISTORTION ); iMeasurement++; - if(dump_regularizaton_details) - MSG("regularization distortion: %g; norm2: %g", err, err*err); + // I REALLY penalize tangential corrections + if(anisotropic) scale *= 10.; + if(Jt) Jrowptr[iMeasurement] = iJacobian; + err = scale*(deltauxy[0]*uxy[1] - deltauxy[1]*uxy[0]); + x[iMeasurement] = err; + norm2_error += err*err; + STORE_JACOBIAN( i_var_intrinsics + Ncore_state + ivar + 0, + scale * uxy[1] * SCALE_DISTORTION ); + STORE_JACOBIAN( i_var_intrinsics + Ncore_state + ivar + 1, + -scale * uxy[0] * SCALE_DISTORTION ); + iMeasurement++; } - } } - - if( modelHasCore_fxfycxcy(&ctx->lensmodel) && - ctx->problem_selections.do_optimize_intrinsics_core ) - for(int icam_intrinsics=0; icam_intrinsicsNcameras_intrinsics; icam_intrinsics++) + else { - 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_selections, &ctx->lensmodel); - - // 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; - - if(Jt) Jrowptr[iMeasurement] = iJacobian; - err = scale_regularization_centerpixel * - (intrinsics_all[icam_intrinsics][2] - cx_target); - x[iMeasurement] = err; - norm2_error += err*err; - 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); - - if(Jt) Jrowptr[iMeasurement] = iJacobian; - err = scale_regularization_centerpixel * - (intrinsics_all[icam_intrinsics][3] - cy_target); - x[iMeasurement] = err; - norm2_error += err*err; - 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); - } - } - } - - - if(ctx->problem_selections.do_apply_regularization_unity_cam01 && - ctx->problem_selections.do_optimize_extrinsics && - ctx->Ncameras_extrinsics > 0) - { - double scale_regularization_unity_cam01 = 0.0; + for(int j=0; jNintrinsics-Ncore; j++) + { + // 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.; + } - // compute scales - { -#warning "triangulated-solve: better unity_cam01 scale" - double normal_unity_cam01_value = 1.0; + if(Jt) Jrowptr[iMeasurement] = iJacobian; + double err = scale*intrinsics_all[icam_intrinsics][j+Ncore]; + x[iMeasurement] = err; + norm2_error += err*err; - double expected_regularization_unity_cam01_error_sq_noscale = - (double)Nmeasurements_regularization_unity_cam01 * - normal_unity_cam01_value * - normal_unity_cam01_value; + STORE_JACOBIAN( i_var_intrinsics + Ncore_state + j, + scale * SCALE_DISTORTION ); - double scale_sq = - expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_unity_cam01_error_sq_noscale; + iMeasurement++; + if(dump_regularizaton_details) + MSG("regularization distortion: %g; norm2: %g", err, err*err); - if(dump_regularizaton_details) - MSG("expected_regularization_unity_cam01_error_sq: %f", expected_regularization_unity_cam01_error_sq_noscale*scale_sq); + } + } + } - scale_regularization_unity_cam01 = sqrt(scale_sq); - } + if( modelHasCore_fxfycxcy(&ctx->lensmodel) && + ctx->problem_selections.do_optimize_intrinsics_core ) + for(int icam_intrinsics=0; icam_intrinsicsNcameras_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_selections, &ctx->lensmodel); - // compute and store regularization terms - { - // I have the pose for the first camera: rt_0r. The distance - // between the origin of this camera and the origin of the - // reference is t_0r - const mrcal_point3_t* t_0r = &camera_rt[0].t; - - const int i_var_extrinsics = - mrcal_state_index_extrinsics(0, - ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, - ctx->Nframes, - ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, - ctx->problem_selections, &ctx->lensmodel); + // 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); - if(Jt) Jrowptr[iMeasurement] = iJacobian; - double err = - scale_regularization_unity_cam01 * - (norm2_vec(3, t_0r->xyz) - 1.); - x[iMeasurement] = err; - norm2_error += err*err; + double err; - for(int i=0; i<3; i++) - STORE_JACOBIAN( i_var_extrinsics+3 + i, - scale_regularization_unity_cam01 * SCALE_TRANSLATION_CAMERA * - 2.* t_0r->xyz[i]); + if(Jt) Jrowptr[iMeasurement] = iJacobian; + err = scale_regularization_centerpixel * + (intrinsics_all[icam_intrinsics][2] - cx_target); + x[iMeasurement] = err; + norm2_error += err*err; + 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); - iMeasurement++; - if(dump_regularizaton_details) - MSG("regularization unity_cam01: %g; norm2: %g", err, err*err); - } + if(Jt) Jrowptr[iMeasurement] = iJacobian; + err = scale_regularization_centerpixel * + (intrinsics_all[icam_intrinsics][3] - cy_target); + x[iMeasurement] = err; + norm2_error += err*err; + 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); + } } } - 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 ) + + // required to indicate the end of the jacobian matrix + if( !ctx->reportFitMsg ) { - MSG("Assertion (iJacobian == ctx->N_j_nonzero ) failed: (%d != %d)", - iJacobian, ctx->N_j_nonzero); - assert(0); + 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)); } } @@ -6191,9 +5157,6 @@ bool mrcal_optimizer_callback(// out int Nobservations_board, int Nobservations_point, - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - // 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 @@ -6203,11 +5166,6 @@ bool mrcal_optimizer_callback(// out // z<0 indicates that this is an outlier const mrcal_point3_t* observations_board_pool, - // Same thing, but for discrete points. Array of shape - // - // ( Nobservations_point,) - const mrcal_point3_t* observations_point_pool, - const mrcal_lensmodel_t* lensmodel, const int* imagersizes, // Ncameras_intrinsics*2 of these @@ -6221,16 +5179,6 @@ bool mrcal_optimizer_callback(// out { bool result = false; - if( ( observations_point_triangulated != NULL && - Nobservations_point_triangulated ) && - ! (!problem_selections.do_optimize_intrinsics_core && - !problem_selections.do_optimize_intrinsics_distortions && - problem_selections.do_optimize_extrinsics ) ) - { - MSG("ERROR: We have triangulated points. At this time this is only allowed if we're NOT optimizing intrinsics AND if we ARE optimizing extrinsics."); - goto done; - } - if( Nobservations_board > 0 ) { if( problem_selections.do_optimize_calobject_warp && calobject_warp == NULL ) @@ -6270,8 +5218,6 @@ bool mrcal_optimizer_callback(// out int Nmeasurements = mrcal_num_measurements(Nobservations_board, Nobservations_point, - observations_point_triangulated, - Nobservations_point_triangulated, calibration_object_width_n, calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, @@ -6282,8 +5228,6 @@ bool mrcal_optimizer_callback(// out int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); int N_j_nonzero = _mrcal_num_j_nonzero(Nobservations_board, Nobservations_point, - observations_point_triangulated, - Nobservations_point_triangulated, calibration_object_width_n, calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, @@ -6318,12 +5262,9 @@ bool mrcal_optimizer_callback(// out .Npoints_fixed = Npoints_fixed, .observations_board = observations_board, .observations_board_pool = observations_board_pool, - .observations_point_pool = observations_point_pool, .Nobservations_board = Nobservations_board, .observations_point = observations_point, .Nobservations_point = Nobservations_point, - .observations_point_triangulated = observations_point_triangulated, - .Nobservations_point_triangulated = Nobservations_point_triangulated, .verbose = verbose, .lensmodel = *lensmodel, .imagersizes = imagersizes, @@ -6396,9 +5337,6 @@ mrcal_optimize( // out int Nobservations_board, int Nobservations_point, - const mrcal_observation_point_triangulated_t* observations_point_triangulated, - int Nobservations_point_triangulated, - // 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 @@ -6412,11 +5350,6 @@ mrcal_optimize( // out // marked with z<0 on output, so this isn't const mrcal_point3_t* observations_board_pool, - // Same thing, but for discrete points. Array of shape - // - // ( Nobservations_point,) - mrcal_point3_t* observations_point_pool, - const mrcal_lensmodel_t* lensmodel, const int* imagersizes, // Ncameras_intrinsics*2 of these mrcal_problem_selections_t problem_selections, @@ -6440,21 +5373,6 @@ mrcal_optimize( // out else problem_selections.do_optimize_calobject_warp = false; - if( ( observations_point_triangulated != NULL && - Nobservations_point_triangulated ) && - ! (!problem_selections.do_optimize_intrinsics_core && - !problem_selections.do_optimize_intrinsics_distortions && - problem_selections.do_optimize_extrinsics ) ) - { - MSG("ERROR: We have triangulated points. At this time this is only allowed if we're NOT optimizing intrinsics AND if we ARE optimizing extrinsics."); - // Because the input to the triangulation routines is unprojected - // vectors, and I don't want to unproject as part of the optimization - // callback. And because I must optimize SOMETHING, so if I have fixed - // intrinsics then the extrinsics cannot be fixed - return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0}; -#warning "triangulated-solve: can I loosen this? Optimizing intrinsics and triangulated points together should work" - } - if(!modelHasCore_fxfycxcy(lensmodel)) problem_selections.do_optimize_intrinsics_core = false; @@ -6500,12 +5418,9 @@ mrcal_optimize( // out .Npoints_fixed = Npoints_fixed, .observations_board = observations_board, .observations_board_pool = observations_board_pool, - .observations_point_pool = observations_point_pool, .Nobservations_board = Nobservations_board, .observations_point = observations_point, .Nobservations_point = Nobservations_point, - .observations_point_triangulated = observations_point_triangulated, - .Nobservations_point_triangulated = Nobservations_point_triangulated, .verbose = verbose, .lensmodel = *lensmodel, .imagersizes = imagersizes, @@ -6516,8 +5431,6 @@ mrcal_optimize( // out .calibration_object_height_n= calibration_object_height_n > 0 ? calibration_object_height_n : 0, .Nmeasurements = mrcal_num_measurements(Nobservations_board, Nobservations_point, - observations_point_triangulated, - Nobservations_point_triangulated, calibration_object_width_n, calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, @@ -6527,8 +5440,6 @@ mrcal_optimize( // out lensmodel), .N_j_nonzero = _mrcal_num_j_nonzero(Nobservations_board, Nobservations_point, - observations_point_triangulated, - Nobservations_point_triangulated, calibration_object_width_n, calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, @@ -6593,17 +5504,25 @@ mrcal_optimize( // out if( !check_gradient ) { - stats.Noutliers_board = 0; - stats.Noutliers_triangulated_point = 0; - - const int Nmeasurements_board = - mrcal_num_measurements_boards(Nobservations_board, - calibration_object_width_n, - calibration_object_height_n); - for(int i=0; ibeforeStep, solver_context); #endif } while( problem_selections.do_apply_outlier_rejection && markOutliers(observations_board_pool, - &stats.Noutliers_board, - &stats.Noutliers_triangulated_point, + &stats.Noutliers, observations_board, Nobservations_board, - Nobservations_point, -#warning "triangulated-solve: not const for now. this will become const once the outlier bit moves to the point_triangulated pool" - (mrcal_observation_point_triangulated_t*)observations_point_triangulated, - Nobservations_point_triangulated, calibration_object_width_n, calibration_object_height_n, solver_context->beforeStep->x, - extrinsics_fromref, verbose) && ({MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again", - stats.Noutliers_board, + stats.Noutliers, Nmeasurements_board, - (double)(stats.Noutliers_board * 100) / (double)Nmeasurements_board); - true;})); -#warning "triangulated-solve: the above print should deal with triangulated points too" - + (double)(stats.Noutliers * 100) / (double)Nmeasurements_board); true;})); // Done. I have the final state. I spit it back out unpack_solver_state( intrinsics, // Ncameras_intrinsics of these @@ -6672,22 +5582,17 @@ mrcal_optimize( // out double regularization_ratio_distortion = 0.0; double regularization_ratio_centerpixel = 0.0; - const int imeas_reg0 = - mrcal_measurement_index_regularization(observations_point_triangulated, - Nobservations_point_triangulated, - calibration_object_width_n, + + int imeas_reg0 = + mrcal_measurement_index_regularization(calibration_object_width_n, calibration_object_height_n, Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, Nobservations_board, Nobservations_point, problem_selections, lensmodel); - - if(imeas_reg0 >= 0 && verbose) + if(problem_selections.do_apply_regularization && imeas_reg0 >= 0) { - // we have regularization - const double* xreg = &solver_context->beforeStep->x[imeas_reg0]; - int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0; int Nmeasurements_regularization_distortion = 0; @@ -6703,6 +5608,8 @@ mrcal_optimize( // out double norm2_err_regularization_distortion = 0; double norm2_err_regularization_centerpixel = 0; + const double* xreg = &solver_context->beforeStep->x[imeas_reg0]; + for(int i=0; ibeforeStep->x[ctx.Nmeasurements]); regularization_ratio_distortion = norm2_err_regularization_distortion / norm2_error; regularization_ratio_centerpixel = norm2_err_regularization_centerpixel / norm2_error; // These are important to the dev, but not to the end user. So I // disable these by default - if(regularization_ratio_distortion > 0.01) - MSG("WARNING: regularization ratio for lens distortion exceeds 1%%. Is the scale factor too high? Ratio = %.3g/%.3g = %.3g", - norm2_err_regularization_distortion, norm2_error, regularization_ratio_distortion); - if(regularization_ratio_centerpixel > 0.01) - MSG("WARNING: regularization ratio for the projection centerpixel exceeds 1%%. Is the scale factor too high? Ratio = %.3g/%.3g = %.3g", - norm2_err_regularization_centerpixel, norm2_error, regularization_ratio_centerpixel); - - double regularization_ratio_unity_cam01 = 0.0; - if(problem_selections.do_apply_regularization_unity_cam01 && - problem_selections.do_optimize_extrinsics && - Ncameras_extrinsics > 0) - { - int Nmeasurements_regularization_unity_cam01 = 1; - double norm2_err_regularization_unity_cam01 = 0; - for(int i=0; i 0.01) - MSG("WARNING: regularization ratio for unity_cam01 exceeds 1%%. Is the scale factor too high? Ratio = %.3g/%.3g = %.3g", - norm2_err_regularization_unity_cam01, norm2_error, regularization_ratio_unity_cam01); - } + // if(regularization_ratio_distortion > 0.01) + // MSG("WARNING: regularization ratio for lens distortion exceeds 1%%. Is the scale factor too high? Ratio = %.3g/%.3g = %.3g", + // norm2_err_regularization_distortion, norm2_error, regularization_ratio_distortion); + // if(regularization_ratio_centerpixel > 0.01) + // MSG("WARNING: regularization ratio for the projection centerpixel exceeds 1%%. Is the scale factor too high? Ratio = %.3g/%.3g = %.3g", + // norm2_err_regularization_centerpixel, norm2_error, regularization_ratio_centerpixel); - assert(xreg == &solver_context->beforeStep->x[ctx.Nmeasurements]); + } - // Disable this by default. Splined models have LOTS of - // parameters, and I don't want to print them. Usually. - // - // for(int i=0; ibeforeStep->x[ctx.Nmeasurements - Nmeasurements_regularization + i]; - // MSG("regularization %d: %f (squared: %f)", i, x, x*x); - // } - MSG("reg err ratio (distortion,centerpixel): %.3g %.3g", - regularization_ratio_distortion, - regularization_ratio_centerpixel); - - if(problem_selections.do_apply_regularization_unity_cam01 && - problem_selections.do_optimize_extrinsics && - Ncameras_extrinsics > 0) + if(verbose) + { + if(problem_selections.do_apply_regularization) { - MSG("reg err ratio (unity_cam01): %.3g", - regularization_ratio_unity_cam01); + // Disable this by default. Splined models have LOTS of + // parameters, and I don't want to print them. Usually. + // + // for(int i=0; ibeforeStep->x[ctx.Nmeasurements - Nmeasurements_regularization + i]; + // MSG("regularization %d: %f (squared: %f)", i, x, x*x); + // } + + MSG("Regularization stats:"); + MSG("reg err ratio (distortion,centerpixel): %.3g %.3g", + regularization_ratio_distortion, + regularization_ratio_centerpixel); } } } @@ -6777,8 +5665,6 @@ mrcal_optimize( // out (dogleg_callback_t*)&optimizer_callback, &ctx); stats.rms_reproj_error__pixels = -#warning "triangulated-solve: not correct for triangulated points either: 1 measurement, not 2" - // THIS IS NOT CORRECT FOR POINTS. I have 3 measurements for points currently sqrt(norm2_error / (double)ctx.Nmeasurements); if(b_packed_final) @@ -6846,6 +5732,3 @@ bool mrcal_write_cameramodel_file(const char* filename, fclose(fp); return result; } - - -#warning "triangulated-solve: fixed points should live in a separate array, instead of at the end of the 'points' array" diff --git a/wpical/src/main/native/thirdparty/mrcal/src/opencv.c b/wpical/src/main/native/thirdparty/mrcal/src/opencv.c deleted file mode 100644 index 3e751f611b1..00000000000 --- a/wpical/src/main/native/thirdparty/mrcal/src/opencv.c +++ /dev/null @@ -1,152 +0,0 @@ -// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. -// Government sponsorship acknowledged. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 - -#include "mrcal.h" - - -// 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 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 - } - } - } - } - } - } -} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp index deb4670887e..f482eb5a210 100644 --- a/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp @@ -26,13 +26,15 @@ rotate_point_r_core(// output // Rodrigues rotation formula: // xrot = x cos(th) + cross(axis, x) sin(th) + axis axist x (1 - cos(th)) // - // I have r = axis*th -> th = mag(r) -> + // I have r = axis*th -> th = norm(r) -> // xrot = x cos(th) + cross(r, x) sin(th)/th + r rt x (1 - cos(th)) / (th*th) - // if inverted: we have r <- -r, axis <- axis, th <- -th - // - // According to the expression above, this only flips the sign on the - // cross() term + // an inversion would flip the sign on: + // - rg + // - cross + // - inner + // But inner is always multiplied by rg, making the sign irrelevant. So an + // inversion only flips the sign on the cross double sign = inverted ? -1.0 : 1.0; const val_withgrad_t th2 = @@ -65,8 +67,7 @@ rotate_point_r_core(// output } else { - // Non-small rotation. This is the normal path. Note that this path is - // still valid even if th ~ 180deg + // Non-small rotation. This is the normal path const val_withgrad_t th = th2.sqrt(); const vec_withgrad_t sc = th.sincos(); @@ -79,6 +80,49 @@ rotate_point_r_core(// output } } +template +static void +r_from_R_core(// output + val_withgrad_t* rg, + + // inputs + const val_withgrad_t* Rg) +{ + val_withgrad_t tr = Rg[0] + Rg[4] + Rg[8]; + val_withgrad_t axis[3] = + { + Rg[2*3 + 1] - Rg[1*3 + 2], + Rg[0*3 + 2] - Rg[2*3 + 0], + Rg[1*3 + 0] - Rg[0*3 + 1] + }; + + val_withgrad_t costh = (tr - 1.) / 2.; + + if( (fabs(axis[0].x) > 1e-10 || + fabs(axis[1].x) > 1e-10 || + fabs(axis[2].x) > 1e-10) && + fabs(costh.x) < (1. - 1e-10) ) + { + // normal path + val_withgrad_t th = costh.acos(); + val_withgrad_t mag_axis_recip = + val_withgrad_t(1.) / + ((axis[0]*axis[0] + + axis[1]*axis[1] + + axis[2]*axis[2]).sqrt()); + for(int i=0; i<3; i++) + rg[i] = axis[i] * mag_axis_recip * th; + } + else + { + // small th. Can't divide by it. But I can look at the limit. + // + // axis / (2 sinth)*th = axis/2 *th/sinth ~ axis/2 + for(int i=0; i<3; i++) + rg[i] = axis[i] / 2.; + } +} + extern "C" void mrcal_rotate_point_r_full( // output double* x_out, // (3,) array @@ -275,6 +319,56 @@ void mrcal_transform_point_rt_full( // output } +extern "C" +void mrcal_r_from_R_full( // output + double* r, // (3,) vector + int r_stride0, // in bytes. <= 0 means "contiguous" + double* J, // (3,3,3) array. Gradient. May be NULL + int J_stride0, // in bytes. <= 0 means "contiguous" + int J_stride1, // in bytes. <= 0 means "contiguous" + int J_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(r, 3); + init_stride_3D(J, 3,3,3); + init_stride_2D(R, 3,3); + + if(J == NULL) + { + vec_withgrad_t<0, 3> rg; + vec_withgrad_t<0, 9> Rg; + Rg.init_vars(&P2(R,0,0), 0,3, -1, R_stride1); + Rg.init_vars(&P2(R,1,0), 3,3, -1, R_stride1); + Rg.init_vars(&P2(R,2,0), 6,3, -1, R_stride1); + + r_from_R_core<0>(rg.v, Rg.v); + rg.extract_value(r, r_stride0); + } + else + { + vec_withgrad_t<9, 3> rg; + vec_withgrad_t<9, 9> Rg; + Rg.init_vars(&P2(R,0,0), 0,3, 0, R_stride1); + Rg.init_vars(&P2(R,1,0), 3,3, 3, R_stride1); + Rg.init_vars(&P2(R,2,0), 6,3, 6, R_stride1); + + r_from_R_core<9>(rg.v, Rg.v); + rg.extract_value(r, r_stride0); + + // J is dr/dR of shape (3,3,3). autodiff.h has a gradient of shape + // (3,9): the /dR part is flattened. I pull it out in 3 chunks that scan + // the middle dimension. So I fill in J[:,0,:] then J[:,1,:] then J[:,2,:] + rg.extract_grad(&P3(J,0,0,0), 0,3, 0,J_stride0,J_stride2,3); + rg.extract_grad(&P3(J,0,1,0), 3,3, 0,J_stride0,J_stride2,3); + rg.extract_grad(&P3(J,0,2,0), 6,3, 0,J_stride0,J_stride2,3); + } +} + template static void compose_r_core(// output @@ -284,161 +378,51 @@ compose_r_core(// output const vec_withgrad_t* r0, const vec_withgrad_t* r1) { - /* - - Described here: - - Altmann, Simon L. "Hamilton, Rodrigues, and the Quaternion Scandal." - Mathematics Magazine, vol. 62, no. 5, 1989, pp. 291–308 - - Available here: - - https://www.jstor.org/stable/2689481 - - I use Equations (19) and (20) on page 302 of this paper. These equations say - that - - R(angle=gamma, axis=n) = - compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) ) - - I need to compute r01 = gamma*n from r0=alpha*l, r1=beta*m; and these are - given as solutions to: - - cos(gamma/2) = - cos(alpha/2)*cos(beta/2) - - sin(alpha/2)*sin(beta/2) * inner(l,m) - sin(gamma/2) n = - sin(alpha/2)*cos(beta/2)*l + - cos(alpha/2)*sin(beta/2)*m + - sin(alpha/2)*sin(beta/2) * cross(l,m) - - For nicer notation, I define + // Described here: + // + // Altmann, Simon L. "Hamilton, Rodrigues, and the Quaternion Scandal." + // Mathematics Magazine, vol. 62, no. 5, 1989, pp. 291–308 + // + // Available here: + // + // https://www.jstor.org/stable/2689481 + // + // I use Equations (19) and (20) on page 302 of this paper. These equations say + // that + // + // R(angle=gamma, axis=n) = + // compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) ) + // + // I need to compute gamma*n, and these are given as solutions to: + // + // cos(gamma/2) = + // cos(alpha/2)*cos(beta/2) - + // sin(alpha/2)*sin(beta/2) * inner(l,m) + // sin(gamma/2) n = + // sin(alpha/2)*cos(beta/2)*l + + // cos(alpha/2)*sin(beta/2)*m + + // sin(alpha/2)*sin(beta/2) * cross(l,m) + // + // For nicer notation, I define + // + // A = alpha/2 + // B = beta /2 + // C = gamma/2 + // + // l = r0 /(2A) + // m = r1 /(2B) + // n = r01/(2C) + // + // I rewrite: + // + // cos(C) = + // cos(A)*cos(B) - + // sin(A)*sin(B) * inner(r0,r1) / 4AB + // sin(C) r01 / 2C = + // sin(A)*cos(B)*r0 / 2A + + // cos(A)*sin(B)*r1 / 2B + + // sin(A)*sin(B) * cross(r0,r1) / 4AB - A = alpha/2 - B = beta /2 - C = gamma/2 - - l = r0 /(2A) - m = r1 /(2B) - n = r01/(2C) - - I rewrite: - - cos(C) = - cos(A)*cos(B) - - sin(A)*sin(B) * inner(r0,r1) / 4AB - sin(C) r01 / 2C = - sin(A)*cos(B)*r0 / 2A + - cos(A)*sin(B)*r1 / 2B + - sin(A)*sin(B) * cross(r0,r1) / 4AB - -> - r01 = - C/sin(C) ( sin(A)/A cos(B)*r0 + - sin(B)/B cos(A)*r1 + - sin(A)/A sin(B)/B * cross(r0,r1) / 2 ) - - I compute cos(C) and then get C and sin(C) and r01 from that - - Note that each r = th*axis has equivalent axis*(k*2*pi + th)*axis and - -axis*(k*2*pi - th) for any integer k. - - Let's confirm that rotating A or B by any number full rotations has no - effect on r01. - - We'll have - - alpha += k*2*pi -> A += k*pi - r0 += r0/mag(r0)*k*2*pi - - if k is even: - sin(A), cos(A) stays the same; - r0 / A -> r0 * (1 + k 2pi/mag(r0)) / (A + k pi) - = r0 * (1 + k 2pi/2A) / (A + k pi) - = r0 * (1 + k pi/A) / (A + k pi) - = r0 / A - - So adding an even number of full rotations produces the same exact r01. If - k is odd (adding an odd number of full rotations; should still produce the - same result) we get - - sin(A), cos(A) -> -sin(A),-cos(A) - r0 / A stays the same, as before - - -> cos(C) and sin(C) r01 / 2C = sin(C) n change sign. - - This means that C -> C +- pi. So an odd k switches to a different mode, - but the meaning of the rotation vector r01 does not change - - - What about shifts of r01? - - Let u = sin(A)/A cos(B)*r0 + - sin(B)/B cos(A)*r1 + - sin(A)/A sin(B)/B * cross(r0,r1) / 2 - - and r01 = C/sinC u - - norm2(u) = - = norm2( sA/A cB 2A l + - sB/B cA 2B m + - sA/A sB/B 4AB cross(l,m) / 2 ) - = norm2( sA cB 2 l + - sB cA 2 m + - sA sB 2 cross(l,m) ) - = 4 (sA cB sA cB + - sB cA sB cA + - 2 sA cB sB cA inner(l,m) + - sA sB sA sB norm2(cross(l,m))) - = 4 (sA cB sA cB + - sB cA sB cA + - 2 sA cB sB cA inner(l,m) + - sA sB sA sB (1 - inner^2(l,m)) ) - - I have - - cC = cA cB - sA sB inner(r0,r1) / 4AB - = cA cB - sA sB inner(l,m) - - So inner(l,m) = (cA cB - cC)/(sA sB) - - norm2(u) = - = 4 (sA cB sA cB + - sB cA sB cA + - 2 sA cB sB cA (cA cB - cC)/(sA sB) + - sA sB sA sB (1 - ((cA cB - cC)/(sA sB))^2) ) - = 4 (sA cB sA cB + - sB cA sB cA + - 2 sA cB sB cA (cA cB - cC)/(sA sB) + - sA sB sA sB (1 - (cA cB - cC)^2/(sA sB)^2) ) - = 4 (sA cB sA cB + - sB cA sB cA + - 2 cB cA cA cB - -2 cB cA cC - sA sB sA sB - -cA cB cA cB - -cC cC + - 2 cA cB cC) - = 4 (sA cB sA cB + - sB cA sB cA + - cB cA cA cB - sA sB sA sB - -cC cC) - = 4 (sA sA + - cA cA + - -cC cC) - = 4 (1 - cC cC) - = 4 sC^2 - - r01 = C/sinC u -> - norm2(r01) = C^2/sin^2(C) norm2(u) - = C^2/sin^2(C) 4 sin^2(C) - = 4 C^2 - So mag(r01) = 2C - - This is what we expect. And any C that satisfies the above expressions will - have mag(r01) = 2C - - */ const val_withgrad_t A = r0->mag() / 2.; const val_withgrad_t B = r1->mag() / 2.; @@ -446,13 +430,7 @@ compose_r_core(// output const val_withgrad_t inner = r0->dot(*r1); const vec_withgrad_t cross = r0->cross(*r1); - // In radians. If my angle is this close to 0, I use the special-case paths - const double eps = 1e-8; - - // Here I special-case A and B near 0. I do this so avoid dividing by 0 in - // the /A and /B expressions. There are no /sin(A) and /sin(B) expressions, - // so I don't need to think about A ~ k pi - if(A.x < eps/2.) + if(A.x < 1e-8) { // A ~ 0. I simplify // @@ -540,7 +518,7 @@ compose_r_core(// output // - inner(r0,r1) (B/tanB - 1) / 4B^2 r1 // + B/tanB r0 // + cross(r0,r1) / 2 - if(B.x < eps) + if(B.x < 1e-8) { // what if B is ALSO near 0? I simplify further // @@ -574,7 +552,7 @@ compose_r_core(// output + cross[i] / 2.; return; } - else if(B.x < eps) + else if(B.x < 1e-8) { // B ~ 0. I simplify // @@ -664,7 +642,7 @@ compose_r_core(// output // + A/tanA r1 // + cross(r0,r1) / 2 - // I don't have an if(A.x < eps){} case here; this is handled in + // I don't have an if(A.x < 1e-8){} case here; this is handled in // the above if() branch const val_withgrad_t& A_over_tanA = A / A.tan(); @@ -692,80 +670,19 @@ compose_r_core(// output cosA*cosB - sinA_over_A*sinB_over_B*inner/4.; - for(int i=0; i<3; i++) - (*r)[i] = - sinA_over_A*cosB*(*r0)[i] + - sinB_over_B*cosA*(*r1)[i] + - sinA_over_A*sinB_over_B*cross[i]/2.; - // To handle numerical fuzz - // cos(x ~ 0) ~ 1 - x^2/2 - if (cosC.x - 1.0 > -eps*eps/2.) - { - // special-case. C ~ 0 -> sin(C)/C ~ 1 -> r01 is already computed. We're - // done - } - // cos(x ~ pi) ~ cos(pi) + (x-pi)^2/2 (-cos(pi)) - // ~ -1 + (x-pi)^2/2 - else if(cosC.x + 1.0 < eps*eps/2. ) - { - // special-case. cos(C) ~ -1 -> C ~ pi. This corresponds to a full - // rotation: gamma = 2C ~ 2pi. I wrap it around to avoid dividing by - // 0. - // - // For any r = gamma n where mag(n)=1 I can use an equivalent r' = - // (gamma-2pi)n - // - // Here gamma = 2C so gamma-2pi = 2(C-pi) - - /* - I have cosC and u = r01 sin(C)/C (stored in the "r" variable) - - gamma = mag(r01) = 2C - mag(u) * C/sin(C) = 2C - mag(u) = 2 sin(C) - - r' = (mag(r01) - 2pi) * r01/mag(r01) - = (1 - 2pi/mag(r01)) * r01 - = (1 - 2pi/2C) * u C/sin(C) - = ((C - pi)/C) * u C/sin(C) - = (C - pi)/sin(C) * u - = -(pi - C)/sin(pi - C) * u - ~ -u - */ - for(int i=0; i<3; i++) - (*r)[i] *= -1.; - } - // Not-strictly-necessary special case. I would like to have C in - // [-pi/2,pi/2] instead of [0,pi]. This will produce a nicer-looking - // (smaller numbers) r01 - else if(cosC.x < 0.) - { - // Need to subtract a rotation - - // I have cos(C) and u (stored in "r") - // r01 = C/sin(C) u - // - // I want r01 - r01/mag(r01)*2pi - // = C/sin(C) u ( 1 - 2pi/mag(r01)) - // = C/sin(C) u ( 1 - pi/C ) - // = 1/sin(C) u ( C - pi ) - // = (C - pi)/sin(C) u - const val_withgrad_t C = cosC.acos(); - const val_withgrad_t sinC = (val_withgrad_t(1.) - cosC*cosC).sqrt(); + if (cosC.x > 1.0) cosC.x = 1.0; + else if(cosC.x < -1.0) cosC.x = -1.0; + const val_withgrad_t C = cosC.acos(); + const val_withgrad_t sinC = (val_withgrad_t(1.) - cosC*cosC).sqrt(); + const val_withgrad_t sinC_over_C_recip = val_withgrad_t(1.) / C.sinx_over_x(sinC); - for(int i=0; i<3; i++) - (*r)[i] *= (C - M_PI) / sinC; - } - else - { - // nominal case - const val_withgrad_t C = cosC.acos(); - const val_withgrad_t sinC = (val_withgrad_t(1.) - cosC*cosC).sqrt(); - - for(int i=0; i<3; i++) - (*r)[i] *= C / sinC; - } + for(int i=0; i<3; i++) + (*r)[i] = + ( sinA_over_A*cosB*(*r0)[i] + + sinB_over_B*cosA*(*r1)[i] + + sinA_over_A*sinB_over_B*cross[i]/2. ) * + sinC_over_C_recip; } extern "C" diff --git a/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c b/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c index 856397b18f7..b17563e9cc1 100644 --- a/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c @@ -15,7 +15,6 @@ #include "poseutils.h" #include "strides.h" -#include "minimath/minimath.h" // All arrays stored in row-major order // @@ -684,13 +683,6 @@ void mrcal_compose_r_tinyr0_gradientr0_full( // output return; } - // I'm computing - // R(angle=gamma, axis=n) = - // compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) ) - // where - // A = alpha/2 - // B = beta /2 - // I have // r01 = r1 // - inner(r0,r1) (B/tanB - 1) / 4B^2 r1 @@ -721,360 +713,3 @@ void mrcal_compose_r_tinyr0_gradientr0_full( // output P2(dr_dr0,2,0) -= -P1(r_1,1)/2.; P2(dr_dr0,2,1) -= P1(r_1,0)/2.; } - -void mrcal_compose_r_tinyr1_gradientr1_full( // output - double* dr_dr1, // (3,3) array; may be NULL - int dr_dr1_stride0, // in bytes. <= 0 means "contiguous" - int dr_dr1_stride1, // in bytes. <= 0 means "contiguous" - - // input - const double* r_0, // (3,) array - int r_0_stride0 // in bytes. <= 0 means "contiguous" - ) -{ - init_stride_2D(dr_dr1, 3, 3); - init_stride_1D(r_0, 3); - - // All the comments and logic appear in compose_r_core() in - // poseutils-uses-autodiff.cc. This is a special-case function with - // manually-computed gradients (because I want to make sure they're fast) - double norm2_r0 = 0.0; - for(int i=0; i<3; i++) - norm2_r0 += P1(r_0,i)*P1(r_0,i); - - if(norm2_r0 < 2e-8*2e-8) - { - // Both vectors are tiny, so I have r01 = r0 + r1, and the gradient is - // an identity matrix - for(int i=0; i<3; i++) - for(int j=0; j<3; j++) - P2(dr_dr1,i,j) = i==j ? 1.0 : 0.0; - return; - } - - // I'm computing - // R(angle=gamma, axis=n) = - // compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) ) - // where - // A = alpha/2 - // B = beta /2 - - // I have - // r01 = r0 - // - inner(r0,r1) (A/tanA - 1) / 4A^2 r0 - // + A/tanA r1 - // + cross(r0,r1) / 2 - // - // I differentiate: - // - // dr01/dr1 = - // - outer(r0,r0) (A/tanA - 1) / 4A^2 - // + A/tanA I - // + skew_symmetric(r0) / 2 - double A = sqrt(norm2_r0) / 2.; - double A_over_tanA = A / tan(A); - - for(int i=0; i<3; i++) - for(int j=0; j<3; j++) - P2(dr_dr1,i,j) = - - P1(r_0,i)*P1(r_0,j) * (A_over_tanA - 1.) / (4.*A*A); - for(int i=0; i<3; i++) - P2(dr_dr1,i,i) += - A_over_tanA; - - P2(dr_dr1,0,1) += -P1(r_0,2)/2.; - P2(dr_dr1,0,2) += P1(r_0,1)/2.; - P2(dr_dr1,1,0) += P1(r_0,2)/2.; - P2(dr_dr1,1,2) += -P1(r_0,0)/2.; - P2(dr_dr1,2,0) += -P1(r_0,1)/2.; - P2(dr_dr1,2,1) += P1(r_0,0)/2.; -} - - -void mrcal_r_from_R_full( // output - double* r, // (3,) vector - int r_stride0, // in bytes. <= 0 means "contiguous" - double* J, // (3,3,3) array. Gradient. May be NULL - int J_stride0, // in bytes. <= 0 means "contiguous" - int J_stride1, // in bytes. <= 0 means "contiguous" - int J_stride2, // in bytes. <= 0 means "contiguous" - - // input - const double* R, // (3,3) array - int R_stride0, // in bytes. <= 0 means "contiguous" - int R_stride1 // in bytes. <= 0 means "contiguous" - ) -{ - init_stride_1D(r, 3); - init_stride_3D(J, 3,3,3); - init_stride_2D(R, 3,3); - - - // Looking at https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula the - // Rodrigues rotation formula for th rad rotation around unit axis v is - // - // R = I + sin(th) V + (1 - cos(th)) V^2 - // - // where V = skew_symmetric(v): - // - // [ 0 -v2 v1] - // V(v) = [ v2 0 -v0] - // [-v1 v0 0] - // - // and - // - // v(V) = [-V12, V02, -V01] - // - // I, V^2 are symmetric; V is anti-symmetric. So R - Rt = 2 sin(th) V - // - // Let's define - // - // [ R21 - R12 ] - // u = [ -R20 + R02 ] = v(R) - v(Rt) - // [ R10 - R01 ] - // - // From the above equations we see that u = 2 sin(th) v. So I compute the - // axis v = u/mag(u). I want th in [0,pi] so I can't compute th from u since - // there's an ambiguity: sin(th) = sin(pi-th). So instead, I compute th from - // trace(R) = 1 + 2*cos(th) - // - // There's an extra wrinkle here. Computing the axis from mag(u) only works - // if sin(th) != 0. So there are two special cases that must be handled: th - // ~ 0 and th ~ 180. If th ~ 0, then the axis doesn't matter and r ~ 0. If - // th ~ 180 then the axis DOES matter, and we need special logic. - const double tr = P2(R,0,0) + P2(R,1,1) + P2(R,2,2); - const double u[3] = - { - P2(R,2,1) - P2(R,1,2), - P2(R,0,2) - P2(R,2,0), - P2(R,1,0) - P2(R,0,1) - }; - - const double costh = (tr - 1.) / 2.; - - // In radians. If my angle is this close to 0, I use the special-case paths - const double eps = 1e-8; - - // near 0 we have norm2u ~ 4 th^2 - const double norm2u = - u[0]*u[0] + - u[1]*u[1] + - u[2]*u[2]; - if(// both conditions to handle roundoff error - norm2u > 4. * eps*eps && - 1. - fabs(costh) > eps*eps/2. ) - { - // normal path - const double th = acos(costh); - for(int i=0; i<3; i++) - P1(r,i) = u[i]/sqrt(norm2u) * th; - } - else if(costh > 0) - { - // small th. Can't divide by it. But I can look at the limit. - // - // u / (2 sinth)*th = u/2 *th/sinth ~ u/2 - for(int i=0; i<3; i++) - P1(r,i) = u[i] / 2.; - } - else - { - // cos(th) < 0. So th ~ +-180 = +-180 + dth where dth ~ 0. And I have - // - // R = I + sin(th) V + (1 - cos(th) ) V^2 - // = I + sin(+-180 + dth) V + (1 - cos(+-180 + dth)) V^2 - // = I - sin(dth) V + (1 + cos(dth)) V^2 - // ~ I - dth V + 2 V^2 - // - // Once again, I, V^2 are symmetric; V is anti-symmetric. So - // - // R - Rt = 2 sin(th) V - // = -2 sin(dth) V - // = -2 dth V - // I want - // - // r = th v - // = dth v +- 180deg v - // - // r = v((R - Rt) / -2.) +- 180deg v - // = u/-2 +- 180deg v - // - // Now we need v; let's look at the symmetric parts: - // - // R + Rt = 2 I + 4 V^2 - //-> V^2 = (R + Rt)/4 - I/2 - // - // [ 0 -v2 v1] - // V(v) = [ v2 0 -v0] - // [-v1 v0 0] - // - // [ -(v1^2+v2^2) v0 v1 v0 v2 ] - // V^2(v) = [ v0 v1 -(v0^2+v2^2) v1 v2 ] - // [ v0 v2 v1 v2 -(v0^2+v1^2) ] - // - // I want v be a unit vector. Can I assume that? From above: - // - // tr(V^2) = -2 norm2(v) - // - // So I want to assume that tr(V^2) = -2. The earlier expression had - // - // R + Rt = 2 I + 4 V^2 - // - // -> tr(R + Rt) = tr(2 I + 4 V^2) - // -> tr(V^2) = (tr(R + Rt) - 6)/4 - // = (2 tr(R) - 6)/4 - // = (1 + 2*cos(th) - 3)/2 - // = -1 + cos(th) - // - // Near th ~ 180deg, this is -2 as required. So we can assume that - // mag(v)=1: - // - // [ v0^2 - 1 v0 v1 v0 v2 ] - // V^2(v) = [ v0 v1 v1^2 - 1 v1 v2 ] - // [ v0 v2 v1 v2 v2^2 - 1 ] - // - // So - // - // v^2 = 1 + diag(V^2) - // = 1 + 2 diag(R)/4 - I/2 - // = 1 + diag(R)/2 - 1/2 - // = (1 + diag(R))/2 - for(int i=0; i<3; i++) - P1(r,i) = u[i] / -2.; - - // Now r += pi v - const double vsq[3] = - { - (P2(R,0,0) + 1.) /2., - (P2(R,1,1) + 1.) /2., - (P2(R,2,2) + 1.) /2. - }; - // This is abs(v) initially - double v[3] = {}; - for(int i=0; i<3; i++) - if(vsq[i] > 0.0) - v[i] = sqrt(vsq[i]); - else - { - // round-off sets this at 0; it's already there. Leave it - } - - // Now I need to get the sign of each individual value. Overall, the - // sign of the vector v doesn't matter. I set the sign of a notably - // non-zero abs(v[i]) to >0, and go from there. - - // threshold can be anything notably > 0. I'd like to encourage the same - // branch to always be taken, so I set the thresholds relatively low - if( v[0] > 0.1) - { - // I leave v[0]>0. - // V^2[0,1] must have the same sign as v1 - // V^2[0,2] must have the same sign as v2 - if( (P2(R,0,1) + P2(R,1,0)) < 0 ) v[1] *= -1.; - if( (P2(R,0,2) + P2(R,2,0)) < 0 ) v[2] *= -1.; - } - else if(v[1] > 0.1) - { - // I leave v[1]>0. - // V^2[1,0] must have the same sign as v0 - // V^2[1,2] must have the same sign as v2 - if( (P2(R,1,0) + P2(R,0,1)) < 0 ) v[0] *= -1.; - if( (P2(R,1,2) + P2(R,2,1)) < 0 ) v[2] *= -1.; - } - else - { - // I leave v[2]>0. - // V^2[2,0] must have the same sign as v0 - // V^2[2,1] must have the same sign as v1 - if( (P2(R,2,0) + P2(R,0,2)) < 0 ) v[0] *= -1.; - if( (P2(R,2,1) + P2(R,1,2)) < 0 ) v[1] *= -1.; - } - - for(int i=0; i<3; i++) - P1(r,i) += v[i] * M_PI; - } - - - if(J != NULL) - { - // Not all (3,3) matrices R are valid rotations, and I make sure to evaluate - // the gradient in the subspace defined by the opposite operation: R_from_r - // - // I'm assuming a flattened R.shape = (9,) everywhere here - // - // - I compute r = r_from_R(R) - // - // - R',dR/dr = R_from_r(r, get_gradients = True) - // R' should match R. This method assumes that. - // - // - We have - // dR = dR/dr dr - // dr = dr/dR dR - // so - // dr/dR dR/dr = I - // - // - dR/dr has shape (9,3). In response to perturbations in r, R moves in a - // rank-3 subspace: this is the local subspace of valid rotation - // matrices. The dr/dR we seek should be limited to that subspace as - // well. So dr/dR = M (dR/dr)' for some 3x3 matrix M - // - // - Combining those two I get - // dr/dR = M (dR/dr)' - // dr/dR dR/dr = M (dR/dr)' dR/dr - // I = M (dR/dr)' dR/dr - // -> - // M = inv( (dR/dr)' dR/dr ) - // -> - // dr/dR = M (dR/dr)' - // = inv( (dR/dr)' dR/dr ) (dR/dr)' - // = pinv(dR/dr) - - // share memory - union - { - // Unused. The tests make sure this is the same as R - double R_roundtrip[3*3]; - double det_inv_dRflat_drT__dRflat_dr[6]; - } m; - - double dRflat_dr[9*3]; // inverse gradient - - mrcal_R_from_r_full( // outputs - m.R_roundtrip,0,0, - dRflat_dr, 0,0,0, - - // input - r,r_stride0 ); - - ////// transpose(dRflat_dr) * dRflat_dr - // 3x3 symmetric matrix; packed,dense storage; row-first - double dRflat_drT__dRflat_dr[6] = {}; - int i_result = 0; - for(int i=0; i<3; i++) - for(int j=i;j<3;j++) - { - for(int k=0; k<9; k++) - dRflat_drT__dRflat_dr[i_result] += - dRflat_dr[k*3 + i]* - dRflat_dr[k*3 + j]; - i_result++; - } - - ////// inv( transpose(dRflat_dr) * dRflat_dr ) - // 3x3 symmetric matrix; packed,dense storage; row-first - double inv_det = 1./cofactors_sym3(dRflat_drT__dRflat_dr, m.det_inv_dRflat_drT__dRflat_dr); - - ////// inv( transpose(dRflat_dr) * dRflat_dr ) transpose(dRflat_dr) - for(int i=0; i<3; i++) - for(int j=0; j<3; j++) - { - // computing dr/dR[i,j] - double dr[3] = {}; - mul_vec3_sym33_vout_scaled( &dRflat_dr[3*(j + 3*i)], m.det_inv_dRflat_drT__dRflat_dr, - dr, - inv_det); - for(int k=0; k<3; k++) - P3(J, k,i,j) = dr[k]; - } - } -} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp b/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp index a6807455b94..29d9b5e5be8 100644 --- a/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp +++ b/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp @@ -100,8 +100,6 @@ triangulate_assume_intersect( // output } -#warning "These all have NGRAD=9, which is inefficient: some/all of the requested gradients could be NULL" - // Basic closest-approach-in-3D routine extern "C" mrcal_point3_t @@ -571,58 +569,46 @@ mrcal_triangulate_leecivera_linf(// outputs return _m; } -// This is called "cheirality" in Lee and Civera's papers -template -static bool chirality(// output - val_withgrad_t& improvement0, - val_withgrad_t& improvement1, - val_withgrad_t& improvement01, - - // input - const val_withgrad_t& l0, - const vec_withgrad_t& v0, - const val_withgrad_t& l1, - const vec_withgrad_t& v1, - const vec_withgrad_t& t01) +static bool chirality(const val_withgrad_t<9 >& l0, + const vec_withgrad_t<9,3>& v0, + const val_withgrad_t<9 >& l1, + const vec_withgrad_t<9,3>& v1, + const vec_withgrad_t<9,3>& t01) { double len2_nominal = 0.0; double len2; - improvement0 = val_withgrad_t(); - improvement1 = val_withgrad_t(); - improvement01 = val_withgrad_t(); + for(int i=0; i<3; i++) + { + double x = ( l1.x*v1.v[i].x + t01.v[i].x) - l0.x*v0.v[i].x; + len2_nominal += x*x; + } + len2 = 0.0; for(int i=0; i<3; i++) { - val_withgrad_t x_nominal = ( l1*v1.v[i] + t01.v[i]) - l0*v0.v[i]; - val_withgrad_t x0 = ( l1*v1.v[i] + t01.v[i]) + l0*v0.v[i]; - val_withgrad_t x1 = (-l1*v1.v[i] + t01.v[i]) - l0*v0.v[i]; - val_withgrad_t x01 = (-l1*v1.v[i] + t01.v[i]) + l0*v0.v[i]; - - improvement0 += x0 *x0 - x_nominal*x_nominal; - improvement1 += x1 *x1 - x_nominal*x_nominal; - improvement01 += x01*x01 - x_nominal*x_nominal; + double x = ( l1.x*v1.v[i].x + t01.v[i].x) + l0.x*v0.v[i].x; + len2 += x*x; } + if( len2 < len2_nominal) return false; - return - improvement0.x > 0.0 && - improvement1.x > 0.0 && - improvement01.x > 0.0; -} + len2 = 0.0; + for(int i=0; i<3; i++) + { + double x = (-l1.x*v1.v[i].x + t01.v[i].x) + l0.x*v0.v[i].x; + len2 += x*x; + } + if( len2 < len2_nominal) return false; -// version without reporting the improvement values -template -static bool chirality(const val_withgrad_t& l0, - const vec_withgrad_t& v0, - const val_withgrad_t& l1, - const vec_withgrad_t& v1, - const vec_withgrad_t& t01) -{ - val_withgrad_t improvement0; - val_withgrad_t improvement1; - val_withgrad_t improvement01; - return chirality(improvement0, improvement1, improvement01, - l0,v0,l1,v1,t01); + len2 = 0.0; + for(int i=0; i<3; i++) + { + double x = (-l1.x*v1.v[i].x + t01.v[i].x) - l0.x*v0.v[i].x; + len2 += x*x; + } + if( len2 < len2_nominal) return false; + + return true; } // The "Mid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier @@ -677,24 +663,6 @@ mrcal_triangulate_leecivera_mid2(// outputs return _m; } - -extern "C" -bool -_mrcal_triangulate_leecivera_mid2_is_convergent(// inputs - - // not-necessarily normalized vectors in the camera-0 - // coord system - const mrcal_point3_t* _v0, - const mrcal_point3_t* _v1, - const mrcal_point3_t* _t01) -{ - mrcal_point3_t p = mrcal_triangulate_leecivera_mid2(NULL,NULL,NULL, - _v0,_v1,_t01); - return !(p.x == 0.0 && - p.y == 0.0 && - p.z == 0.0); -} - // The "wMid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and // Javier Civera. https://arxiv.org/abs/1907.11917 extern "C" @@ -752,267 +720,3 @@ mrcal_triangulate_leecivera_wmid2(// outputs return _m; } - -static -val_withgrad_t<6> -angle_error__assume_small(const vec_withgrad_t<6,3>& v0, - const vec_withgrad_t<6,3>& v1) -{ - const val_withgrad_t<6> inner00 = v0.norm2(); - const val_withgrad_t<6> inner11 = v1.norm2(); - const val_withgrad_t<6> inner01 = v0.dot(v1); - - val_withgrad_t<6> costh = inner01 / (inner00*inner11).sqrt(); - if(costh.x < 0.0) - // Could happen with barely-divergent rays - costh *= val_withgrad_t<6>(-1.0); - - // The angle is assumed small, so cos(th) ~ 1 - th*th/2. - // -> th ~ sqrt( 2*(1 - cos(th)) ) - val_withgrad_t<6> th_sq = costh*(-2.) + 2.; - - -#warning "triangulated-solve: temporary hack to avoid dividing by 0" - if(th_sq.x < 1e-21) - { - return val_withgrad_t<6>(); - } - - - if(th_sq.x < 0) - // To handle roundoff errors - th_sq.x = 0; - - return th_sq.sqrt(); -#warning "triangulated-solve: look at numerical issues that will results in sqrt(<0)" -#warning "triangulated-solve: look at behavior near 0 where dsqrt/dx -> inf" -} - -#warning "triangulated-solve: maybe exposing the triangulated-error C function is OK? I'm already exposing the Python function" - -__attribute__((unused)) -static -double relu(double x, double knee) -{ - /* Smooth function ~ x>0 ? x : 0 - - Three modes - - x < 0: 0 - - 0 < x < knee: k*x^2 - - knee < x: x + eps - - At the transitions I want the function and the first derivative - to be continuous. At the knee I want d/dx = 1. So 2*k*knee = 1 -> - k = 1/(2*knee). k * knee^2 = knee + eps -> eps = knee * (k*knee - - 1) = -knee/2 - */ - if(x <= 0) return 0.0; - if(knee <= x) return x - knee/2.0; - - double k = 1. / (2*knee); - return k * x*x; -} - -static -val_withgrad_t<6> sigmoid(val_withgrad_t<6> x, double knee) -{ - /* Smooth function maps to 0..1 - - Modes - - x < 0: 0 - - 0 < x < knee: smooth interpolation - - knee < x: 1 - */ - if(x.x <= 0 ) return 0.0; - if(knee <= x.x) return 1.0; - - // transition at (x - knee/2.) < 0 - // f(x) = a x^2 + b x + c - // f(-knee/2.) = 0 - // f(0) = 1/2 - // f'(-knee/2.) = 0 - if(x.x < knee/2.0) - { - double b = 2./knee; - double a = b/knee; - double c = 1./2.; - return c + (x.x - knee/2.)*(b + (x.x - knee/2.)*a); - } - - // transition at (x - knee/2.) > 0 - // f(x) = a x^2 + b x + c - // f(knee/2.) = 1 - // f'(knee/2.) = 0 - // f(0) = 1/2 - { - double b = 2./knee; - double a = -b/knee; - double c = 1./2.; - return c + (x.x - knee/2.)*(b + (x.x - knee/2.)*a); - } -} - -// Internal function used in the optimization. This uses -// mrcal_triangulate_leecivera_mid2(), but contains logic in the divergent-ray -// case more appropriate for the optimization loop -extern "C" -double -_mrcal_triangulated_error(// outputs - mrcal_point3_t* _derr_dv1, - mrcal_point3_t* _derr_dt01, - - // inputs - - // not-necessarily normalized vectors in the camera-0 - // coord system - const mrcal_point3_t* _v0, - const mrcal_point3_t* _v1, - const mrcal_point3_t* _t01) -{ - ////////////////////////// Copy of mrcal_triangulate_leecivera_mid2(). I - ////////////////////////// extend it - - // Implementation here is a bit different: I don't propagate the gradient in - // respect to v0 - - // The paper has m0, m1 as the cam1-frame observation vectors. I do - // everything in cam0-frame - vec_withgrad_t<6,3> v0 (_v0 ->xyz, -1); // No gradient. Hopefully the - // compiler will collapse this - // aggressively - vec_withgrad_t<6,3> v1 (_v1 ->xyz, 0); - vec_withgrad_t<6,3> t01(_t01->xyz, 3); - - val_withgrad_t<6> p_norm2_recip = val_withgrad_t<6>(1.0) / cross_norm2<6>(v0, v1); - - val_withgrad_t<6> l0 = (cross_norm2<6>(v1, t01) * p_norm2_recip).sqrt(); - val_withgrad_t<6> l1 = (cross_norm2<6>(v0, t01) * p_norm2_recip).sqrt(); - - vec_withgrad_t<6,3> m = (v0*l0 + t01+v1*l1) / 2.0; - - // I compute the angle between the triangulated point and one of the - // observation rays, and I double this to measure from ray to ray - - // This is a fit error, which should be small. A small-angle cos() - // approximation is valid, unless the models don't fit at all. In which - // case a cos() error is the least of our issues - val_withgrad_t<6> err = angle_error__assume_small( v0, m ) * 2.; - -#warning "triangulated-solve: what happens when the rays are exactly parallel? Make sure the numerics remain happy. They don't: I divide by cross(v0,v1) ~ 0" - -#if 0 - // original method - if(!chirality(l0, v0, l1, v1, t01)) - { - // The rays diverge. This is aphysical, but an incorrect (i.e. - // not-yet-converged) geometry can cause this. Even if the optimization - // has converged, this can still happen if pixel noise or an incorrect - // feature association bumped converging rays to diverge. - // - // An obvious thing do wo would be to return the distance to the - // vanishing point. This creates a yaw bias however: barely convergent - // rays have zero effect on yaw, but barely divergent rays have a strong - // effect on yaw - // - // Goals: - // - // - There should be no qualitative change in the cost function as rays - // cross over from convergent to divergent. Low-error, parallel-ish - // rays look out to infinity, which means that these define yaw very - // poorly, and would affect the pitch, roll only. Yaw is what controls - // divergence, so if random noise makes rays diverge, we should use - // the error as before, to set our pitch, roll - // - // - Very divergent rays are bogus, and I do apply a penalty factor - // based on the divergence. This penalty factor begins to kick in only - // past a certain point, so there's no effect at the transition. This - // transition point should be connected to the outlier rejection - // threshold. - val_withgrad_t<6> err_to_vanishing_point = angle_error__assume_small(v0, v1); - - // I have three modes: - // - // - err_to_vanishing_point < THRESHOLD_DIVERGENT_LOWER - // I attribute the error to random noise, and simply report the - // reprojection error as before, ignoring the divergence. This will - // barely affect the yaw, but will pull the pitch and roll in the - // desired directions - // - // - err_to_vanishing_point > THRESHOLD_DIVERGENT_UPPER - // I add a term to pull the observation towards the vanishing point. - // This affects the yaw primarily, and does not touch the pitch and - // roll very much, since I don't have reliable information about them - // here - // - // - err_to_vanishing_point between THRESHOLD_DIVERGENT_LOWER and _UPPER - // I linearly the scale on this divergence term from 0 to 1 - -#warning "triangulated-solve: set reasonable thresholds" -#define THRESHOLD_DIVERGENT_LOWER 3.0e-4 -#define THRESHOLD_DIVERGENT_UPPER 6.0e-4 - - if(err_to_vanishing_point.x <= THRESHOLD_DIVERGENT_LOWER) - { - // We're barely divergent. Just do the normal thing - } - else if(err_to_vanishing_point.x >= THRESHOLD_DIVERGENT_UPPER) - { - // we're VERY divergent. Add another cost term: - // the distance to the vanishing point -#warning "triangulated-solve: temporary testing logic" -#if defined DIVERGENT_COST_ONLY && DIVERGENT_COST_ONLY - err = err_to_vanishing_point; -#else - err += err_to_vanishing_point; -#endif - err.x += 200.0; - } - else - { - // linearly interpolate. As - // err_to_vanishing_point lower->upper we - // produce k 0->1 - val_withgrad_t<6> k = - (err_to_vanishing_point - THRESHOLD_DIVERGENT_LOWER) / - (THRESHOLD_DIVERGENT_UPPER - THRESHOLD_DIVERGENT_LOWER); - -#warning "triangulated-solve: temporary testing logic" -#if defined DIVERGENT_COST_ONLY && DIVERGENT_COST_ONLY - err = k*err_to_vanishing_point + (val_withgrad_t<6>(1.0)-k)*err; -#else - err += k*err_to_vanishing_point; -#endif - err.x += 100.0; - } - } - -#else - - // new method - val_withgrad_t<6> improvement0; - val_withgrad_t<6> improvement1; - val_withgrad_t<6> improvement01; - if(!chirality(improvement0, improvement1, improvement01, - l0, v0, l1, v1, t01)) - { - val_withgrad_t<6> err_to_vanishing_point = angle_error__assume_small(v0, v1); - - err += - err_to_vanishing_point * (sigmoid(-improvement0, 3.0) + - sigmoid(-improvement1, 3.0) + - sigmoid(-improvement01, 3.0)); - } - -#endif - - if(_derr_dv1 != NULL) - memcpy(_derr_dv1->xyz, - &err.j[0], - 3*sizeof(double)); - if(_derr_dt01 != NULL) - memcpy(_derr_dt01->xyz, - &err.j[3], - 3*sizeof(double)); - - return err.x; -}