diff --git a/.github/actions/pregen/action.yml b/.github/actions/pregen/action.yml index 2c61be413ac..3c251432bff 100644 --- a/.github/actions/pregen/action.yml +++ b/.github/actions/pregen/action.yml @@ -11,10 +11,10 @@ runs: - name: Install jinja and protobuf run: python -m pip install jinja2 protobuf grpcio-tools shell: bash - - name: Install protobuf dependencies + - name: Install protobuf and perl dependencies run: | sudo apt-get update - sudo apt-get install -y protobuf-compiler + sudo apt-get install -y protobuf-compiler liblist-moreutils-perl wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf-1.3.3-linux-x86_64.exe chmod +x protoc-gen-quickbuf-1.3.3-linux-x86_64.exe shell: bash @@ -45,6 +45,10 @@ runs: ./wpilibj/generate_pwm_motor_controllers.py shell: bash + - name: Regenerate mrcal minimath + run: ./wpical/generate_mrcal.py + shell: bash + - name: Regenerate wpimath run: | ./wpimath/generate_nanopb.py diff --git a/CMakeLists.txt b/CMakeLists.txt index f689f3af0ac..9448fc87bfa 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ cmake_dependent_option( ) option(WITH_CSCORE "Build cscore (needs OpenCV)" ON) option(WITH_NTCORE "Build ntcore" ON) +option(WITH_WPICAL "Build wpical" OFF) option(WITH_WPIMATH "Build wpimath" ON) cmake_dependent_option( WITH_WPIUNITS @@ -142,6 +143,11 @@ if(WITH_DOCS) include(AddDoxygenDocs) add_doxygen_docs() endif() + +if(WITH_WPICAL) + find_package(Ceres CONFIG REQUIRED) +endif() + find_package(LIBSSH CONFIG 0.7.1) set(CMAKE_FIND_PACKAGE_PREFER_CONFIG ON) @@ -314,6 +320,9 @@ if(WITH_GUI) add_subdirectory(glass) add_subdirectory(outlineviewer) add_subdirectory(sysid) + if(WITH_WPICAL) + add_subdirectory(wpical) + endif() if(LIBSSH_FOUND) add_subdirectory(roborioteamnumbersetter) add_subdirectory(datalogtool) diff --git a/README.md b/README.md index af2fcf37ffa..35760470061 100644 --- a/README.md +++ b/README.md @@ -59,6 +59,8 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen On macOS ARM, run `softwareupdate --install-rosetta`. This is necessary to be able to use the macOS x86 roboRIO toolchain on ARM. +On linux, run `sudo apt install gfortran`. This is necessary to be able to build WPIcal on linux platforms. + ## Setup Clone the WPILib repository and follow the instructions above for installing any required tooling. The build process uses versioning information from git. Downloading the source is not sufficient to run the build. diff --git a/settings.gradle b/settings.gradle index 298d00149f1..b81e6280b83 100644 --- a/settings.gradle +++ b/settings.gradle @@ -62,6 +62,7 @@ include 'epilogue-processor' include 'epilogue-runtime' include 'thirdparty:googletest' include 'thirdparty:imgui_suite' +include 'wpical' buildCache { def cred = { diff --git a/shared/ceres.gradle b/shared/ceres.gradle new file mode 100644 index 00000000000..550fd6c9e79 --- /dev/null +++ b/shared/ceres.gradle @@ -0,0 +1,13 @@ +nativeUtils { + nativeDependencyContainer { + ceres(getNativeDependencyTypeClass('WPIStaticMavenDependency')) { + groupId = "edu.wpi.first.thirdparty.frc2024.ceres" + artifactId = "ceres-cpp" + headerClassifier = "headers" + sourceClassifier = "sources" + ext = "zip" + version = '2.2-3' + targetPlatforms.addAll(nativeUtils.wpi.platforms.desktopPlatforms) + } + } +} diff --git a/upstream_utils/libdogleg.py b/upstream_utils/libdogleg.py new file mode 100755 index 00000000000..68ab38326a5 --- /dev/null +++ b/upstream_utils/libdogleg.py @@ -0,0 +1,58 @@ +#!/usr/bin/env python3 + +import os +import shutil + +from upstream_utils import Lib, walk_cwd_and_copy_if + + +def copy_upstream_src(wpilib_root): + wpical = os.path.join(wpilib_root, "wpical") + + # Delete old install + for d in [ + "src/main/native/thirdparty/libdogleg/src", + "src/main/native/thirdparty/libdogleg/include", + ]: + shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + + files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith("dogleg.h"), + os.path.join(wpical, "src/main/native/thirdparty/libdogleg/include"), + ) + 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.cpp"), + 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") + content = content.replace("__attribute__((unused))", "") + content = content.replace( + "#include ", "#include " + ) + with open(f, "w") as file: + file.write(content) + + +def main(): + name = "libdogleg" + url = "https://github.com/dkogan/libdogleg" + tag = "c971ea43088d286a3683c1039b9a85f761f7df15" + + libdogleg = Lib(name, url, tag, copy_upstream_src) + libdogleg.main() + + +if __name__ == "__main__": + main() diff --git a/upstream_utils/libdogleg_patches/0001-Convert-to-C.patch b/upstream_utils/libdogleg_patches/0001-Convert-to-C.patch new file mode 100644 index 00000000000..fa82ac501ec --- /dev/null +++ b/upstream_utils/libdogleg_patches/0001-Convert-to-C.patch @@ -0,0 +1,38 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 29 Nov 2024 19:52:22 -0500 +Subject: [PATCH 1/3] Convert to C++ + +--- + dogleg.c => dogleg.cpp | 0 + dogleg.h | 8 +++++++- + 2 files changed, 7 insertions(+), 1 deletion(-) + rename dogleg.c => dogleg.cpp (100%) + +diff --git a/dogleg.c b/dogleg.cpp +similarity index 100% +rename from dogleg.c +rename to dogleg.cpp +diff --git a/dogleg.h b/dogleg.h +index 74337263a89d3448f55e125db6702ebfa7588b97..4a23b669a30642d290549bb94a1c787f079d0647 100644 +--- a/dogleg.h ++++ b/dogleg.h +@@ -126,7 +126,9 @@ typedef struct + + } dogleg_solverContext_t; + +- ++#ifdef __cplusplus ++extern "C" { ++#endif + // Fills in the given structure with the default parameter set + void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters); + +@@ -293,3 +295,7 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu + int NoutlierFeatures, + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx); ++ ++#ifdef __cplusplus ++} // extern "C" ++#endif diff --git a/upstream_utils/libdogleg_patches/0002-Replace-VLAs-with-vectors.patch b/upstream_utils/libdogleg_patches/0002-Replace-VLAs-with-vectors.patch new file mode 100644 index 00000000000..d41eeae0d76 --- /dev/null +++ b/upstream_utils/libdogleg_patches/0002-Replace-VLAs-with-vectors.patch @@ -0,0 +1,79 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 29 Nov 2024 19:54:54 -0500 +Subject: [PATCH 2/3] Replace VLAs with vectors + +--- + dogleg.cpp | 17 +++++++++-------- + 1 file changed, 9 insertions(+), 8 deletions(-) + +diff --git a/dogleg.cpp b/dogleg.cpp +index 7e4259c303e21a9f4c63ba16f1bf5df131935057..9ed95f64b232f41a51fe23d72885ecadd86dc065 100644 +--- a/dogleg.cpp ++++ b/dogleg.cpp +@@ -6,6 +6,7 @@ + // Apparently I need this in MSVC to get constants + #define _USE_MATH_DEFINES + ++#include + #include + #include + #include +@@ -1907,7 +1908,7 @@ static bool getOutliernessFactors_dense( // output + // where A = Jo inv(JtJ) Jot + // + // A is symmetric. I store the upper triangle +- double A[featureSize*(featureSize+1)/2]; ++ std::vector A(featureSize*(featureSize+1)/2); + int iA=0; + for(int i=0; ix[i_measurement], +- A, featureSize, *scale); ++ A.data(), featureSize, *scale); + } + + result = true; +@@ -2008,7 +2009,7 @@ static bool getOutliernessFactors_sparse( // output + // where A = Jo inv(JtJ) Jot + // + // A is symmetric. I store the upper triangle +- double A[featureSize*(featureSize+1)/2]; ++ std::vector A(featureSize*(featureSize+1)/2); + int iA=0; + for(int i=0; ix[i_measurement], +- A, featureSize, *scale); ++ A.data(), featureSize, *scale); + } + + result = true; +@@ -2212,8 +2213,8 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu + + // This is Jt because cholmod thinks in terms of col-first instead of + // row-first +- int Jt_p[featureSize+1]; +- int Jt_i[NstateActive*featureSize]; ++ std::vector Jt_p(featureSize+1); ++ std::vector Jt_i(NstateActive*featureSize); + for(int i=0; i<=featureSize; i++) + { + Jt_p[i] = i*NstateActive; +@@ -2224,8 +2225,8 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu + cholmod_sparse Jt_query_sparse = {.nrow = ctx->Nstate, + .ncol = featureSize, + .nzmax = NstateActive*featureSize, +- .p = (void*)Jt_p, +- .i = (void*)Jt_i, ++ .p = (void*)Jt_p.data(), ++ .i = (void*)Jt_i.data(), + .x = (double*)JqueryFeature, + .sorted = 1, + .packed = 1, diff --git a/upstream_utils/libdogleg_patches/0003-Fix-C-build-errors.patch b/upstream_utils/libdogleg_patches/0003-Fix-C-build-errors.patch new file mode 100644 index 00000000000..c0ab181158b --- /dev/null +++ b/upstream_utils/libdogleg_patches/0003-Fix-C-build-errors.patch @@ -0,0 +1,324 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Sun, 1 Dec 2024 00:28:14 -0500 +Subject: [PATCH 3/3] Fix C++ build errors + +--- + dogleg.cpp | 110 +++++++++++++++++++++++++++++++---------------------- + 1 file changed, 65 insertions(+), 45 deletions(-) + +diff --git a/dogleg.cpp b/dogleg.cpp +index 9ed95f64b232f41a51fe23d72885ecadd86dc065..c93f696a9427f0644b047308d2d99302cd246462 100644 +--- a/dogleg.cpp ++++ b/dogleg.cpp +@@ -327,9 +327,9 @@ void _dogleg_testGradient(unsigned int var, const double* p0, + { + int is_sparse = NJnnz > 0; + +- double* x0 = malloc(Nmeas * sizeof(double)); +- double* x = malloc(Nmeas * sizeof(double)); +- double* p = malloc(Nstate * sizeof(double)); ++ double* x0 = static_cast(malloc(Nmeas * sizeof(double))); ++ double* x = static_cast(malloc(Nmeas * sizeof(double))); ++ double* p = static_cast(malloc(Nstate * sizeof(double))); + ASSERT(x0); + ASSERT(x); + ASSERT(p); +@@ -376,8 +376,8 @@ void _dogleg_testGradient(unsigned int var, const double* p0, + } + else + { +- J_dense = malloc( Nmeas * Nstate * sizeof(J_dense[0]) ); +- J_dense0 = malloc( Nmeas * Nstate * sizeof(J_dense[0]) ); ++ J_dense = static_cast(malloc( Nmeas * Nstate * sizeof(J_dense[0]) )); ++ J_dense0 = static_cast(malloc( Nmeas * Nstate * sizeof(J_dense[0]) )); + + dogleg_callback_dense_t* f_dense = (dogleg_callback_dense_t*)f; + p[var] -= GRADTEST_DELTA/2.0; +@@ -487,11 +487,13 @@ static void computeCauchyUpdate(dogleg_operatingPoint_t* point, + + // LAPACK prototypes for a packed cholesky factorization and a linear solve + // using that factorization, respectively ++extern "C" { + int dpptrf_(char* uplo, int* n, double* ap, + int* info, int uplo_len); + int dpptrs_(char* uplo, int* n, int* nrhs, + double* ap, double* b, int* ldb, int* info, + int uplo_len); ++} + + + void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx) +@@ -538,8 +540,8 @@ void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solve + if(ctx->factorization_dense == NULL) + { + // Need to store symmetric JtJ, so I only need one triangle of it +- ctx->factorization_dense = malloc( ctx->Nstate * (ctx->Nstate+1) / 2 * +- sizeof( ctx->factorization_dense[0])); ++ ctx->factorization_dense = static_cast(malloc( ctx->Nstate * (ctx->Nstate+1) / 2 * ++ sizeof( ctx->factorization_dense[0]))); + ASSERT(ctx->factorization_dense); + } + +@@ -582,7 +584,9 @@ void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solve + + + int info; +- dpptrf_(&(char){'L'}, &(int){ctx->Nstate}, ctx->factorization_dense, ++ char uplo = 'L'; ++ int Nstate_copy = ctx->Nstate; ++ dpptrf_(&uplo, &Nstate_copy, ctx->factorization_dense, + &info, 1); + ASSERT(info >= 0); // we MUST either succeed or see complain of singular + // JtJ +@@ -611,10 +615,10 @@ static void computeGaussNewtonUpdate(dogleg_operatingPoint_t* point, dogleg_solv + if( ctx->is_sparse ) + { + // solve JtJ*updateGN = Jt*x. Gauss-Newton step is then -updateGN +- cholmod_dense Jt_x_dense = {.nrow = ctx->Nstate, ++ cholmod_dense Jt_x_dense = {.nrow = static_cast(ctx->Nstate), + .ncol = 1, +- .nzmax = ctx->Nstate, +- .d = ctx->Nstate, ++ .nzmax = static_cast(ctx->Nstate), ++ .d = static_cast(ctx->Nstate), + .x = point->Jt_x, + .xtype = CHOLMOD_REAL, + .dtype = CHOLMOD_DOUBLE}; +@@ -626,18 +630,22 @@ static void computeGaussNewtonUpdate(dogleg_operatingPoint_t* point, dogleg_solv + ctx->factorization, + &Jt_x_dense, + &ctx->common); +- vec_negate(point->updateGN_cholmoddense->x, ++ vec_negate(static_cast(point->updateGN_cholmoddense->x), + ctx->Nstate); // should be more efficient than this later + +- point->updateGN_lensq = norm2(point->updateGN_cholmoddense->x, ctx->Nstate); ++ point->updateGN_lensq = norm2(static_cast(point->updateGN_cholmoddense->x), ctx->Nstate); + } + else + { + memcpy( point->updateGN_dense, point->Jt_x, ctx->Nstate * sizeof(point->updateGN_dense[0])); + int info; +- dpptrs_(&(char){'L'}, &(int){ctx->Nstate}, &(int){1}, ++ char uplo = 'L'; ++ int nhrs = 1; ++ int Nstate_copy = ctx->Nstate; ++ int Nstate_copy2 = ctx->Nstate; ++ dpptrs_(&uplo, &Nstate_copy, &nhrs, + ctx->factorization_dense, +- point->updateGN_dense, &(int){ctx->Nstate}, &info, 1); ++ point->updateGN_dense, &Nstate_copy2, &info, 1); + vec_negate(point->updateGN_dense, + ctx->Nstate); // should be more efficient than this later + +@@ -677,7 +685,7 @@ static void computeInterpolatedUpdate(double* update_dogleg, + double dsq = trustregion*trustregion; + double norm2a = point->updateCauchy_lensq; + const double* a = point->updateCauchy; +- const double* b = ctx->is_sparse ? point->updateGN_cholmoddense->x : point->updateGN_dense; ++ const double* b = ctx->is_sparse ? static_cast(point->updateGN_cholmoddense->x) : point->updateGN_dense; + + double l2 = 0.0; + double neg_c = 0.0; +@@ -1029,7 +1037,7 @@ dogleg_operatingPoint_t* allocOperatingPoint(int Nstate, int numMeasurements, + { + int is_sparse = NJnnz > 0; + +- dogleg_operatingPoint_t* point = malloc(sizeof(dogleg_operatingPoint_t)); ++ dogleg_operatingPoint_t* point = static_cast(malloc(sizeof(dogleg_operatingPoint_t))); + ASSERT(point != NULL); + + +@@ -1042,7 +1050,7 @@ dogleg_operatingPoint_t* allocOperatingPoint(int Nstate, int numMeasurements, + if(!is_sparse) + Npool += numMeasurements*Nstate + Nstate; + +- double* pool = malloc( Npool * sizeof(double) ); ++ double* pool = static_cast(malloc( Npool * sizeof(double) )); + ASSERT( pool != NULL ); + + point->p = &pool[0]; +@@ -1170,7 +1178,7 @@ static double _dogleg_optimize(double* p, unsigned int Nstate, + int is_sparse = NJnnz > 0; + + +- dogleg_solverContext_t* ctx = malloc(sizeof(dogleg_solverContext_t)); ++ dogleg_solverContext_t* ctx = static_cast(malloc(sizeof(dogleg_solverContext_t))); + ctx->f = f; + ctx->cookie = cookie; + ctx->factorization = NULL; +@@ -1294,10 +1302,13 @@ static bool pseudoinverse_J_dense(// output + memcpy(out, + &point->J_dense[i_meas0*ctx->Nstate], + NmeasInChunk*ctx->Nstate*sizeof(double)); +- dpptrs_(&(char){'L'}, &(int){ctx->Nstate}, &NmeasInChunk, ++ char uplo = 'L'; ++ int Nstate_copy = ctx->Nstate; ++ int Nstate_copy2 = ctx->Nstate; ++ dpptrs_(&uplo, &Nstate_copy, &NmeasInChunk, + ctx->factorization_dense, + out, +- &(int){ctx->Nstate}, &info, 1); ++ &Nstate_copy2, &info, 1); + return info==0; + } + +@@ -1873,11 +1884,12 @@ static bool getOutliernessFactors_dense( // output + int Nmeasurements = ctx->Nmeasurements; + bool result = false; + +- double* invJtJ_Jt = malloc(Nstate*chunk_size*sizeof(double)); ++ double* invJtJ_Jt = static_cast(malloc(Nstate*chunk_size*sizeof(double))); + if(invJtJ_Jt == NULL) + { + SAY("Couldn't allocate invJtJ_Jt!"); +- goto done; ++ free(invJtJ_Jt); ++ return result; + } + + getOutliernessScale(scale, +@@ -1895,7 +1907,8 @@ static bool getOutliernessFactors_dense( // output + if(!pinvresult) + { + SAY("Couldn't compute pinv!"); +- goto done; ++ free(invJtJ_Jt); ++ return result; + } + i_measurement_valid_chunk_start = i_measurement; + i_measurement_valid_chunk_last = i_measurement+chunk_size-1; +@@ -1926,7 +1939,6 @@ static bool getOutliernessFactors_dense( // output + } + + result = true; +- done: + free(invJtJ_Jt); + return result; + } +@@ -1975,7 +1987,9 @@ static bool getOutliernessFactors_sparse( // output + if(!Jt_chunk) + { + SAY("Couldn't allocate Jt_chunk!"); +- goto done; ++ if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); ++ if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); ++ return result; + } + + getOutliernessScale(scale, +@@ -1995,7 +2009,9 @@ static bool getOutliernessFactors_sparse( // output + if(invJtJ_Jt == NULL) + { + SAY("Couldn't compute pinv!"); +- goto done; ++ if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); ++ if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); ++ return result; + } + + i_measurement_valid_chunk_start = i_measurement; +@@ -2032,7 +2048,6 @@ static bool getOutliernessFactors_sparse( // output + } + + result = true; +- done: + if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + return result; +@@ -2222,18 +2237,18 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu + for(int j=0; jNstate, +- .ncol = featureSize, +- .nzmax = NstateActive*featureSize, +- .p = (void*)Jt_p.data(), +- .i = (void*)Jt_i.data(), +- .x = (double*)JqueryFeature, +- .sorted = 1, +- .packed = 1, ++ cholmod_sparse Jt_query_sparse = {.nrow = static_cast(ctx->Nstate), ++ .ncol = static_cast(featureSize), ++ .nzmax = static_cast(NstateActive*featureSize), ++ .p = Jt_p.data(), ++ .i = Jt_i.data(), ++ .x = const_cast(JqueryFeature), + .stype = 0, // NOT symmetric + .itype = CHOLMOD_INT, + .xtype = CHOLMOD_REAL, +- .dtype = CHOLMOD_DOUBLE}; ++ .dtype = CHOLMOD_DOUBLE, ++ .sorted = 1, ++ .packed = 1}; + + // Really shouldn't need to do this every time. In fact I probably don't need + // to do it at all, since this will have been done by the solver during the +@@ -2408,18 +2423,22 @@ bool dogleg_markOutliers(// output, input + + bool markedAny = false; + +- double* factors = malloc(Nfeatures * sizeof(double)); ++ double* factors = static_cast(malloc(Nfeatures * sizeof(double))); + if(factors == NULL) + { + SAY("Error allocating factors"); +- goto done; ++ free(factors); ++ return markedAny; + } + + if(!dogleg_getOutliernessFactors(factors, scale, + featureSize, Nfeatures, + *Noutliers, + point, ctx)) +- goto done; ++ { ++ free(factors); ++ return markedAny; ++ } + + // I have my list of POTENTIAL outliers (any that have factor > 1.0). I + // check to see how much confidence I would lose if I were to throw out any +@@ -2427,7 +2446,10 @@ bool dogleg_markOutliers(// output, input + // is acceptable + double confidence0 = getConfidence(-1); + if( confidence0 < 0.0 ) +- goto done; ++ { ++ free(factors); ++ return markedAny; ++ } + + SAY_IF_VERBOSE("Initial confidence: %g", confidence0); + +@@ -2466,7 +2488,6 @@ bool dogleg_markOutliers(// output, input + } + } + +- done: + free(factors); + return markedAny; + } +@@ -2490,11 +2511,11 @@ void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude), + if(featureSize <= 1) + featureSize = 1; + +- double* factors = malloc(Nfeatures * sizeof(double)); ++ double* factors = static_cast(malloc(Nfeatures * sizeof(double))); + if(factors == NULL) + { + SAY("Error allocating factors"); +- goto done; ++ free(factors); + } + + dogleg_getOutliernessFactors(factors, scale, +@@ -2516,6 +2537,5 @@ void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude), + rot_confidence_drop_relative); + } + +- done: + free(factors); + } diff --git a/upstream_utils/mrcal.py b/upstream_utils/mrcal.py new file mode 100755 index 00000000000..be6f57d7c60 --- /dev/null +++ b/upstream_utils/mrcal.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 + +import os +import shutil + +from upstream_utils import Lib, walk_cwd_and_copy_if + + +def copy_upstream_src(wpilib_root): + wpical = os.path.join(wpilib_root, "wpical") + + # Delete old install + for d in [ + "src/main/native/thirdparty/mrcal/src", + "src/main/native/thirdparty/mrcal/include", + ]: + shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + + files = walk_cwd_and_copy_if( + lambda dp, f: (f.endswith(".h") or f.endswith(".hh")) + and not f.endswith("heap.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"), + ) + files = files + walk_cwd_and_copy_if( + lambda dp, f: ( + f.endswith(".c") + or f.endswith(".cc") + or f.endswith(".cpp") + or f.endswith(".pl") + ) + and not f.endswith("heap.cc") + and not f.endswith("mrcal-pywrap.c") + and not f.endswith("image.c") + and not f.endswith("stereo.c") + and not f.endswith("stereo-matching-libelas.cc") + and not f.endswith("uncertainty.c") + and not f.endswith("traverse-sensor-links.c") + and not dp.startswith(os.path.join(".", "doc")) + and not dp.startswith(os.path.join(".", "test")), + os.path.join(wpical, "src/main/native/thirdparty/mrcal/src"), + ) + + for f in files: + with open(f) as file: + content = file.read() + content = content.replace("#warning", "// #warning") + content = content.replace('__attribute__ ((visibility ("hidden")))', "") + content = content.replace("__attribute__((unused))", "") + with open(f, "w") as file: + file.write(content) + + +def main(): + name = "mrcal" + url = "https://github.com/dkogan/mrcal" + tag = "662a539d3cbba4948c31d06a780569173b3fb6e6" + + mrcal = Lib(name, url, tag, copy_upstream_src) + mrcal.main() + + +if __name__ == "__main__": + main() diff --git a/upstream_utils/mrcal_java.py b/upstream_utils/mrcal_java.py new file mode 100755 index 00000000000..5fdd639b16b --- /dev/null +++ b/upstream_utils/mrcal_java.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 + +import os +import shutil + +from upstream_utils import Lib, walk_cwd_and_copy_if + + +def delete_lines_by_range(file_path, start_line, end_line): + # Read all lines from the file + with open(file_path, "r") as file: + lines = file.readlines() + + # Filter out lines that are within the specified range + filtered_lines = [ + line + for i, line in enumerate(lines, start=1) + if not (start_line <= i <= end_line) + ] + + # Write the remaining lines back to the file + with open(file_path, "w") as file: + file.writelines(filtered_lines) + + +def copy_upstream_src(wpilib_root): + wpical = os.path.join(wpilib_root, "wpical") + + # Delete old install + for d in [ + "src/main/native/thirdparty/mrcal_java/src", + "src/main/native/thirdparty/mrcal_java/include", + ]: + shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) + + os.chdir("src") + files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith("mrcal_wrapper.h"), + os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/include"), + ) + + files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith("mrcal_wrapper.cpp"), + os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/src"), + ) + + for f in files: + with open(f) as file: + content = file.read() + content = content.replace("#include ", "") + content = content.replace( + "// mrcal_point3_t *c_observations_point_pool = observations_point;", + "mrcal_point3_t *c_observations_point_pool = observations_point;", + ) + content = content.replace( + "// mrcal_observation_point_triangulated_t *observations_point_triangulated =", + "mrcal_observation_point_triangulated_t *observations_point_triangulated = NULL;", + ) + content = content.replace( + "// observations_point_triangulated,", + "observations_point_triangulated,", + ) + content = content.replace( + "// 0, // hard-coded to 0", "0, // hard-coded to 0" + ) + content = content.replace( + "// observations_point_triangulated, -1,", + "observations_point_triangulated, -1,", + ) + content = content.replace( + "c_observations_board_pool, &mrcal_lensmodel, c_imagersizes,", + "c_observations_board_pool, c_observations_point_pool, &mrcal_lensmodel, c_imagersizes,", + ) + with open(f, "w") as file: + file.write(content) + + +def main(): + name = "mrcal_java" + url = "https://github.com/PhotonVision/mrcal-java" + tag = "5f9d3168ccf1ecdfca48da13ea07fffa47f95d00" + + mrcal_java = Lib(name, url, tag, copy_upstream_src) + mrcal_java.main() + + +if __name__ == "__main__": + main() diff --git a/upstream_utils/mrcal_java_patches/0001-Support-new-mrcal-version.patch b/upstream_utils/mrcal_java_patches/0001-Support-new-mrcal-version.patch new file mode 100644 index 00000000000..26a7777b6c6 --- /dev/null +++ b/upstream_utils/mrcal_java_patches/0001-Support-new-mrcal-version.patch @@ -0,0 +1,22 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Sat, 30 Nov 2024 17:52:37 -0500 +Subject: [PATCH] Support new mrcal version + +--- + src/mrcal_wrapper.cpp | 2 +- + 1 file changed, 1 insertion(+), 1 deletion(-) + +diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp +index 569f5928ba0b389e40476cbfc224a0c09ecd5814..3a9a5519e670b0c867281f1e05aa2046b938fa8d 100644 +--- a/src/mrcal_wrapper.cpp ++++ b/src/mrcal_wrapper.cpp +@@ -212,7 +212,7 @@ static std::unique_ptr mrcal_calibrate( + std::vector residuals = {c_x_final, c_x_final + Nmeasurements}; + return std::make_unique( + true, intrinsics, stats.rms_reproj_error__pixels, residuals, +- calobject_warp, stats.Noutliers); ++ calobject_warp, stats.Noutliers_board); + } + + struct MrcalSolveOptions { diff --git a/upstream_utils/mrcal_patches/0001-Fix-MSVC-build-errors.patch b/upstream_utils/mrcal_patches/0001-Fix-MSVC-build-errors.patch new file mode 100644 index 00000000000..cc4a3b3c6ad --- /dev/null +++ b/upstream_utils/mrcal_patches/0001-Fix-MSVC-build-errors.patch @@ -0,0 +1,155 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Mon, 11 Nov 2024 00:15:05 -0500 +Subject: [PATCH 1/8] Fix MSVC build errors + +--- + autodiff.hh | 4 ++-- + mrcal-internal.h | 22 +++++++++++----------- + mrcal-types.h | 26 ++++++++++---------------- + mrcal.c | 9 --------- + 4 files changed, 23 insertions(+), 38 deletions(-) + +diff --git a/autodiff.hh b/autodiff.hh +index b6f217354c9de45493d50f776082fe2151330a1d..37aa8c5a048a538ccdf500a43bbd8d788c953a05 100644 +--- a/autodiff.hh ++++ b/autodiff.hh +@@ -26,7 +26,7 @@ template + struct val_withgrad_t + { + double x; +- double j[NGRAD]; ++ double j[NGRAD == 0 ? 1 : NGRAD]; + + __attribute__ ((visibility ("hidden"))) + val_withgrad_t(double _x = 0.0) : x(_x) +@@ -281,7 +281,7 @@ struct val_withgrad_t + template + struct vec_withgrad_t + { +- val_withgrad_t v[NVEC]; ++ val_withgrad_t v[NVEC == 0 ? 1 : NVEC]; + + vec_withgrad_t() {} + +diff --git a/mrcal-internal.h b/mrcal-internal.h +index 4ec6acadb5fc92ec264fb460e1fcc882dd3face3..8fc2857758ef7763cc1ef5b79a495aec7b7bc1bd 100644 +--- a/mrcal-internal.h ++++ b/mrcal-internal.h +@@ -12,16 +12,16 @@ + // wrapper only + + // These models have no precomputed data +-typedef struct {} mrcal_LENSMODEL_PINHOLE__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_LONLAT__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_LATLON__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV4__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV5__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV8__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV12__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_CAHVOR__precomputed_t; +-typedef struct {} mrcal_LENSMODEL_CAHVORE__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__precomputed_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_CAHVORE__precomputed_t; + + // The splined stereographic models configuration parameters can be used to + // compute the segment size. I cache this computation +@@ -38,7 +38,7 @@ typedef struct + union + { + #define PRECOMPUTED_STRUCT(s,n) mrcal_ ##s##__precomputed_t s##__precomputed; +- MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT); ++ MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT) + #undef PRECOMPUTED_STRUCT + }; + } mrcal_projection_precomputed_t; +diff --git a/mrcal-types.h b/mrcal-types.h +index e5cf6c637f8f2bf9e4bbe4ad443661159c511887..7e43319b882c3e29eb3429f95c77fc8bd664ce1a 100644 +--- a/mrcal-types.h ++++ b/mrcal-types.h +@@ -45,24 +45,18 @@ + + + // parametric models have no extra configuration +-typedef struct {} mrcal_LENSMODEL_PINHOLE__config_t; +-typedef struct {} mrcal_LENSMODEL_STEREOGRAPHIC__config_t; +-typedef struct {} mrcal_LENSMODEL_LONLAT__config_t; +-typedef struct {} mrcal_LENSMODEL_LATLON__config_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV4__config_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV5__config_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV8__config_t; +-typedef struct {} mrcal_LENSMODEL_OPENCV12__config_t; +-typedef struct {} mrcal_LENSMODEL_CAHVOR__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__config_t; ++typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__config_t; + + #define _MRCAL_ITEM_DEFINE_ELEMENT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) type name bitfield; + +-#ifndef __cplusplus +-// This barfs with g++ 4.8, so I disable it for C++ in general. Checking it for +-// C code is sufficient +-_Static_assert(sizeof(uint16_t) == sizeof(unsigned short int), "I need a short to be 16-bit. Py_BuildValue doesn't let me just specify that. H means 'unsigned short'"); +-#endif +- + // Configuration for CAHVORE. These are given as an an + // "X macro": https://en.wikipedia.org/wiki/X_Macro + #define MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_, cookie) \ +@@ -128,7 +122,7 @@ typedef struct + union + { + #define CONFIG_STRUCT(s,n) mrcal_ ##s##__config_t s##__config; +- MRCAL_LENSMODEL_LIST(CONFIG_STRUCT); ++ MRCAL_LENSMODEL_LIST(CONFIG_STRUCT) + #undef CONFIG_STRUCT + }; + } mrcal_lensmodel_t; +diff --git a/mrcal.c b/mrcal.c +index d1195a36050e5cbde393ed033e4d5699ce52225a..85ab4e26a5ab487fc13e66ebd486bd7c00480c05 100644 +--- a/mrcal.c ++++ b/mrcal.c +@@ -2674,8 +2674,6 @@ void project( // out + if(!camera_at_identity) + { + // make sure I can pass mrcal_pose_t.r as an rt[] transformation +- _Static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure"); +- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure"); + mrcal_compose_rt( _joint_rt, + gg._d_rj_rc, gg._d_rj_rf, + gg._d_tj_rc, gg._d_tj_tf, +@@ -3465,8 +3463,6 @@ void mrcal_pack_solver_state_vector( // out, in + lensmodel, problem_selections, + Ncameras_intrinsics ); + +- _Static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure"); +- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure"); + if( problem_selections.do_optimize_extrinsics ) + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + { +@@ -3715,11 +3711,6 @@ void mrcal_unpack_solver_state_vector( // out, in + + if( problem_selections.do_optimize_extrinsics ) + { +- _Static_assert( offsetof(mrcal_pose_t, r) == 0, +- "mrcal_pose_t has expected structure"); +- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), +- "mrcal_pose_t has expected structure"); +- + mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&b[i_state]); + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &b[i_state] ); diff --git a/upstream_utils/mrcal_patches/0002-Make-mrcal-functions-extern-C.patch b/upstream_utils/mrcal_patches/0002-Make-mrcal-functions-extern-C.patch new file mode 100644 index 00000000000..7f5afcad1da --- /dev/null +++ b/upstream_utils/mrcal_patches/0002-Make-mrcal-functions-extern-C.patch @@ -0,0 +1,34 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 22 Nov 2024 09:54:00 -0500 +Subject: [PATCH 2/8] Make mrcal functions extern "C" + +--- + mrcal.h | 7 ++++++- + 1 file changed, 6 insertions(+), 1 deletion(-) + +diff --git a/mrcal.h b/mrcal.h +index 1eef334a79597cfae61f8111ab97ca04f02b83b2..488ddfe72b3407b3fcb7c3d9de43d1a07433744d 100644 +--- a/mrcal.h ++++ b/mrcal.h +@@ -20,7 +20,9 @@ + //////////////////////////////////////////////////////////////////////////////// + //////////////////// Lens models + //////////////////////////////////////////////////////////////////////////////// +- ++#ifdef __cplusplus ++extern "C" { ++#endif + // Return an array of strings listing all the available lens models + // + // These are all "unconfigured" strings that use "..." placeholders for any +@@ -846,6 +848,9 @@ void mrcal_free_cameramodel(mrcal_cameramodel_t** cameramodel); + bool mrcal_write_cameramodel_file(const char* filename, + const mrcal_cameramodel_t* cameramodel); + ++#ifdef __cplusplus ++} // extern "C" ++#endif + #define DECLARE_mrcal_apply_color_map(T,Tname) \ + bool mrcal_apply_color_map_##Tname( \ + mrcal_image_bgr_t* out, \ diff --git a/upstream_utils/mrcal_patches/0003-Make-mrcal-C.patch b/upstream_utils/mrcal_patches/0003-Make-mrcal-C.patch new file mode 100644 index 00000000000..4180945cd37 --- /dev/null +++ b/upstream_utils/mrcal_patches/0003-Make-mrcal-C.patch @@ -0,0 +1,14 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 22 Nov 2024 09:55:22 -0500 +Subject: [PATCH 3/8] Make mrcal C++ + +--- + mrcal.c => mrcal.cpp | 0 + 1 file changed, 0 insertions(+), 0 deletions(-) + rename mrcal.c => mrcal.cpp (100%) + +diff --git a/mrcal.c b/mrcal.cpp +similarity index 100% +rename from mrcal.c +rename to mrcal.cpp diff --git a/upstream_utils/mrcal_patches/0004-Replace-VLAs-with-vectors.patch b/upstream_utils/mrcal_patches/0004-Replace-VLAs-with-vectors.patch new file mode 100644 index 00000000000..454c796e9fc --- /dev/null +++ b/upstream_utils/mrcal_patches/0004-Replace-VLAs-with-vectors.patch @@ -0,0 +1,284 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Fri, 29 Nov 2024 22:56:02 -0500 +Subject: [PATCH 4/8] Replace VLAs with vectors + +--- + mrcal.cpp | 101 +++++++++++++++++++++++++++--------------------------- + 1 file changed, 51 insertions(+), 50 deletions(-) + +diff --git a/mrcal.cpp b/mrcal.cpp +index 85ab4e26a5ab487fc13e66ebd486bd7c00480c05..5f28f3303da4b4669af896587427b2ad3ad4b7e6 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -7,6 +7,7 @@ + // http://www.apache.org/licenses/LICENSE-2.0 + + // Apparently I need this in MSVC to get constants ++#include + #define _USE_MATH_DEFINES + + #include +@@ -28,7 +29,7 @@ + + #define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0) + +- ++#define restrict __restrict + + // Returns a static string, using "..." as a placeholder for any configuration + // values +@@ -1141,7 +1142,7 @@ void project_cahvor( // outputs + double s__dmu_dalphabeta__domega_dalphabeta = dmu_dtau * s__dtau_dalphabeta__domega_dalphabeta; + + double dpdistorted_dpcam[3*3] = {}; +- double dpdistorted_ddistortion[3*NdistortionParams]; ++ std::vector dpdistorted_ddistortion(3*NdistortionParams); + + for(int i=0; i<3; i++) + { +@@ -2918,7 +2919,7 @@ bool _mrcal_project_internal( // out + .t = p[i]}; + + // simple non-intrinsics-gradient path +- double dq_dintrinsics_pool_double[Ngradients]; ++ std::vector dq_dintrinsics_pool_double(Ngradients); + int dq_dintrinsics_pool_int [1]; + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; +@@ -2926,7 +2927,7 @@ bool _mrcal_project_internal( // out + + project( &q[i], + +- dq_dintrinsics_pool_double, ++ dq_dintrinsics_pool_double.data(), + dq_dintrinsics_pool_int, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + +@@ -3943,21 +3944,21 @@ bool mrcal_corresponding_icam_extrinsics(// out + return false; + } + +- int icam_map_to_extrinsics[Ncameras_intrinsics]; +- int icam_map_to_intrinsics[Ncameras_extrinsics+1]; ++ std::vector icam_map_to_extrinsics(Ncameras_intrinsics); ++ std::vector icam_map_to_intrinsics(Ncameras_extrinsics+1); + for(int i=0; iNcameras_intrinsics][ctx->Nintrinsics]; +- mrcal_pose_t camera_rt[ctx->Ncameras_extrinsics]; ++ std::vector> intrinsics_all(ctx->Ncameras_intrinsics, std::vector(ctx->Nintrinsics)); ++ std::vector camera_rt(ctx->Ncameras_extrinsics); + + mrcal_calobject_warp_t calobject_warp_local = {}; + const int i_var_calobject_warp = +@@ -4669,12 +4670,12 @@ void optimizer_callback(// input state + + // these are computed in respect to the real-unit parameters, + // NOT the unit-scale parameters used by the optimizer +- mrcal_point3_t dq_drcamera [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_point3_t dq_dtcamera [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_point3_t dq_drframe [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_point3_t dq_dtframe [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_calobject_warp_t dq_dcalobject_warp[ctx->calibration_object_width_n*ctx->calibration_object_height_n][2]; +- mrcal_point2_t q_hypothesis [ctx->calibration_object_width_n*ctx->calibration_object_height_n]; ++ std::vector dq_drcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dtcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_drframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dtframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dcalobject_warp(ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector q_hypothesis (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + // I get the intrinsics gradients in separate arrays, possibly sparsely. + // All the data lives in dq_dintrinsics_pool_double[], with the other data + // indicating the meaning of the values in the pool. +@@ -4686,35 +4687,35 @@ void optimizer_callback(// input state + // this case explicitly here. I store dx/dfx and dy/dfy; no cross terms + int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics); + +- double dq_dintrinsics_pool_double[ctx->calibration_object_width_n*ctx->calibration_object_height_n*Ngradients]; +- int dq_dintrinsics_pool_int [ctx->calibration_object_width_n*ctx->calibration_object_height_n]; ++ std::vector dq_dintrinsics_pool_double(ctx->calibration_object_width_n*ctx->calibration_object_height_n*Ngradients); ++ std::vector dq_dintrinsics_pool_int (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; + + int splined_intrinsics_grad_irun = 0; + +- project(q_hypothesis, ++ project(q_hypothesis.data(), + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? +- dq_dintrinsics_pool_double : NULL, ++ dq_dintrinsics_pool_double.data() : NULL, + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? +- dq_dintrinsics_pool_int : NULL, ++ dq_dintrinsics_pool_int.data() : NULL, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + + ctx->problem_selections.do_optimize_extrinsics ? +- (mrcal_point3_t*)dq_drcamera : NULL, ++ dq_drcamera.data() : NULL, + ctx->problem_selections.do_optimize_extrinsics ? +- (mrcal_point3_t*)dq_dtcamera : NULL, ++ dq_dtcamera.data() : NULL, + ctx->problem_selections.do_optimize_frames ? +- (mrcal_point3_t*)dq_drframe : NULL, ++ dq_drframe.data() : NULL, + ctx->problem_selections.do_optimize_frames ? +- (mrcal_point3_t*)dq_dtframe : NULL, ++ dq_dtframe.data() : NULL, + has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ? +- (mrcal_calobject_warp_t*)dq_dcalobject_warp : NULL, ++ dq_dcalobject_warp.data() : NULL, + + // input +- intrinsics_all[icam_intrinsics], ++ intrinsics_all[icam_intrinsics].data(), + &camera_rt[icam_extrinsics], &frame_rt, + ctx->calobject_warp == NULL ? NULL : &calobject_warp_local, + icam_extrinsics < 0, +@@ -4800,43 +4801,43 @@ void optimizer_callback(// input state + if( icam_extrinsics >= 0 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, +- dq_drcamera[i_pt][i_xy].xyz[0] * ++ dq_drcamera[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_ROTATION_CAMERA, +- dq_drcamera[i_pt][i_xy].xyz[1] * ++ dq_drcamera[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_ROTATION_CAMERA, +- dq_drcamera[i_pt][i_xy].xyz[2] * ++ dq_drcamera[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_ROTATION_CAMERA); + STORE_JACOBIAN3( i_var_camera_rt + 3, +- dq_dtcamera[i_pt][i_xy].xyz[0] * ++ dq_dtcamera[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_TRANSLATION_CAMERA, +- dq_dtcamera[i_pt][i_xy].xyz[1] * ++ dq_dtcamera[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_TRANSLATION_CAMERA, +- dq_dtcamera[i_pt][i_xy].xyz[2] * ++ dq_dtcamera[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_TRANSLATION_CAMERA); + } + + if( ctx->problem_selections.do_optimize_frames ) + { + STORE_JACOBIAN3( i_var_frame_rt + 0, +- dq_drframe[i_pt][i_xy].xyz[0] * ++ dq_drframe[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_ROTATION_FRAME, +- dq_drframe[i_pt][i_xy].xyz[1] * ++ dq_drframe[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_ROTATION_FRAME, +- dq_drframe[i_pt][i_xy].xyz[2] * ++ dq_drframe[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_ROTATION_FRAME); + STORE_JACOBIAN3( i_var_frame_rt + 3, +- dq_dtframe[i_pt][i_xy].xyz[0] * ++ dq_dtframe[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_TRANSLATION_FRAME, +- dq_dtframe[i_pt][i_xy].xyz[1] * ++ dq_dtframe[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_TRANSLATION_FRAME, +- dq_dtframe[i_pt][i_xy].xyz[2] * ++ dq_dtframe[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_TRANSLATION_FRAME); + } + + if( has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ) + { + STORE_JACOBIAN_N( i_var_calobject_warp, +- dq_dcalobject_warp[i_pt][i_xy].values, ++ dq_dcalobject_warp[i_pt * 2 + i_xy].values, + weight * SCALE_CALOBJECT_WARP, + MRCAL_NSTATE_CALOBJECT_WARP); + } +@@ -5061,7 +5062,7 @@ void optimizer_callback(// input state + int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics); + + // WARNING: "compute size(dq_dintrinsics_pool_double) correctly and maybe bounds-check" +- double dq_dintrinsics_pool_double[Ngradients]; ++ std::vector dq_dintrinsics_pool_double(Ngradients); + // used for LENSMODEL_SPLINED_STEREOGRAPHIC only, but getting rid of + // this in other cases isn't worth the trouble + int dq_dintrinsics_pool_int [1]; +@@ -5081,7 +5082,7 @@ void optimizer_callback(// input state + project(&q_hypothesis, + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? +- dq_dintrinsics_pool_double : NULL, ++ dq_dintrinsics_pool_double.data() : NULL, + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_int : NULL, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, +@@ -5095,7 +5096,7 @@ void optimizer_callback(// input state + NULL, + + // input +- intrinsics_all[icam_intrinsics], ++ intrinsics_all[icam_intrinsics].data(), + &camera_rt[icam_extrinsics], + + // I only have the point position, so the 'rt' memory +@@ -6590,8 +6591,8 @@ mrcal_optimize( // out + + // WARNING: is it reasonable to put this on the stack? Can I use + // b_packed_final for this? +- double packed_state[Nstate]; +- pack_solver_state(packed_state, ++ std::vector packed_state(Nstate); ++ pack_solver_state(packed_state.data(), + lensmodel, intrinsics, + extrinsics_fromref, + frames_toref, +@@ -6628,7 +6629,7 @@ mrcal_optimize( // out + if(solver_context != NULL) + dogleg_freeContext(&solver_context); + +- norm2_error = dogleg_optimize2(packed_state, ++ norm2_error = dogleg_optimize2(packed_state.data(), + Nstate, ctx.Nmeasurements, ctx.N_j_nonzero, + (dogleg_callback_t*)&optimizer_callback, &ctx, + &dogleg_parameters, +@@ -6683,7 +6684,7 @@ mrcal_optimize( // out + frames_toref, // Nframes of these + points, // Npoints of these + calobject_warp, +- packed_state, ++ packed_state.data(), + lensmodel, + problem_selections, + Ncameras_intrinsics, Ncameras_extrinsics, +@@ -6793,7 +6794,7 @@ mrcal_optimize( // out + } + else + for(int ivar=0; ivar +Date: Fri, 29 Nov 2024 22:56:22 -0500 +Subject: [PATCH 5/8] Use immediately invoked lambda + +--- + mrcal.cpp | 4 ++-- + 1 file changed, 2 insertions(+), 2 deletions(-) + +diff --git a/mrcal.cpp b/mrcal.cpp +index 5f28f3303da4b4669af896587427b2ad3ad4b7e6..e048bef1d1b5addfd137d3c492108c36e56eda7f 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -6668,11 +6668,11 @@ mrcal_optimize( // out + solver_context->beforeStep->x, + extrinsics_fromref, + verbose) && +- ({MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again", ++ ([=]{MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again", + stats.Noutliers_board, + Nmeasurements_board, + (double)(stats.Noutliers_board * 100) / (double)Nmeasurements_board); +- true;})); ++ return true;}())); + #if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS + #warning "triangulated-solve: the above print should deal with triangulated points too" + #endif diff --git a/upstream_utils/mrcal_patches/0006-Fix-MSVC-build-errors.patch b/upstream_utils/mrcal_patches/0006-Fix-MSVC-build-errors.patch new file mode 100644 index 00000000000..c585a886376 --- /dev/null +++ b/upstream_utils/mrcal_patches/0006-Fix-MSVC-build-errors.patch @@ -0,0 +1,434 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Gold856 <117957790+Gold856@users.noreply.github.com> +Date: Wed, 4 Dec 2024 00:58:00 -0500 +Subject: [PATCH 6/8] Fix MSVC build errors + +--- + minimath/minimath-extra.h | 1 + + mrcal.cpp | 57 ++++++----- + poseutils.c | 209 -------------------------------------- + 3 files changed, 32 insertions(+), 235 deletions(-) + +diff --git a/minimath/minimath-extra.h b/minimath/minimath-extra.h +index 2930a5d6aa47af29a3255342838d679032ee4562..194b0ccf22c47a8d693eee24f6f563a2d651f338 100644 +--- a/minimath/minimath-extra.h ++++ b/minimath/minimath-extra.h +@@ -5,6 +5,7 @@ + // replacement, and I'm not going to be thorough and I'm not going to add tests + // until I do that. + ++#define restrict __restrict + + // Upper triangle is stored, in the usual row-major order. + __attribute__((unused)) +diff --git a/mrcal.cpp b/mrcal.cpp +index e048bef1d1b5addfd137d3c492108c36e56eda7f..1560cd106ff5bcfe49476f785aa8108119318b3a 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -2742,11 +2742,10 @@ void project( // out + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config = + &lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config; + *gradient_sparse_meta = +- (gradient_sparse_meta_t) + { +- .run_side_length = config->order+1, +- .ivar_stridey = 2*config->Nx, +- .pool = &dq_dintrinsics_pool_double[ivar_pool] ++ &dq_dintrinsics_pool_double[ivar_pool], ++ static_cast(config->order+1), ++ static_cast(2*config->Nx), + }; + ivar_pool += Npoints*2 * runlen; + } +@@ -2765,10 +2764,11 @@ void project( // out + + if( calibration_object_width_n == 0 ) + { // projecting discrete points ++ mrcal_point3_t pt_ref = {}; // Unused + mrcal_point3_t p = + _propagate_extrinsics( _dp_drc,_dp_dtc,_dp_drf,_dp_dtf, + &dp_drc,&dp_dtc,&dp_drf,&dp_dtf, +- &(mrcal_point3_t){}, ++ &pt_ref, + camera_at_identity ? NULL : &gg, + Rj, d_Rj_rj, &joint_rt[3]); + _project_point( q, +@@ -3227,10 +3227,11 @@ bool _mrcal_unproject_internal( // out + // MSG("init. q=(%g,%g)", q[i].x, q[i].y); + + // initial estimate: pinhole projection ++ const mrcal_point3_t v = {.x = (q[i].x-cx)/fx, ++ .y = (q[i].y-cy)/fy, ++ .z = 1.}; + mrcal_project_stereographic( (mrcal_point2_t*)out->xyz, NULL, +- &(mrcal_point3_t){.x = (q[i].x-cx)/fx, +- .y = (q[i].y-cy)/fy, +- .z = 1.}, ++ &v, + 1, + intrinsics ); + // MSG("init. out->xyz[]=(%g,%g)", out->x, out->y); +@@ -4670,10 +4671,10 @@ void optimizer_callback(// input state + + // these are computed in respect to the real-unit parameters, + // NOT the unit-scale parameters used by the optimizer +- std::vector dq_drcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); +- std::vector dq_dtcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); +- std::vector dq_drframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); +- std::vector dq_dtframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_drcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dtcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_drframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); ++ std::vector dq_dtframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_dcalobject_warp(ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector q_hypothesis (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + // I get the intrinsics gradients in separate arrays, possibly sparsely. +@@ -6242,7 +6243,7 @@ bool mrcal_optimizer_callback(// out + 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; ++ return result; + } + + if( Nobservations_board > 0 ) +@@ -6250,7 +6251,7 @@ bool mrcal_optimizer_callback(// out + if( problem_selections.do_optimize_calobject_warp && calobject_warp == NULL ) + { + MSG("ERROR: We're optimizing the calibration object warp, so a buffer with a seed MUST be passed in."); +- goto done; ++ return result; + } + } + else +@@ -6266,7 +6267,7 @@ bool mrcal_optimizer_callback(// out + !problem_selections.do_optimize_calobject_warp) + { + MSG("Not optimizing any of our variables!"); +- goto done; ++ return result; + } + + +@@ -6279,7 +6280,7 @@ bool mrcal_optimizer_callback(// out + { + MSG("The buffer passed to fill-in b_packed has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nstate*(int)sizeof(double),buffer_size_b_packed); +- goto done; ++ return result; + } + + int Nmeasurements = mrcal_num_measurements(Nobservations_board, +@@ -6312,7 +6313,7 @@ bool mrcal_optimizer_callback(// out + { + MSG("The buffer passed to fill-in x has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nmeasurements*(int)sizeof(double),buffer_size_x); +- goto done; ++ return result; + } + + const int Npoints_fromBoards = +@@ -6332,9 +6333,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, ++ .observations_point_pool = observations_point_pool, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, +@@ -6367,7 +6368,6 @@ bool mrcal_optimizer_callback(// out + + result = true; + +-done: + return result; + } + +@@ -6516,9 +6516,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, ++ .observations_point_pool = observations_point_pool, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, +@@ -6635,10 +6635,13 @@ mrcal_optimize( // out + &dogleg_parameters, + &solver_context); + +- if(norm2_error < 0) ++ if(norm2_error < 0) { + // libdogleg barfed. I quit out +- goto done; ++ if(solver_context != NULL) ++ dogleg_freeContext(&solver_context); + ++ return stats; ++ } + #if 0 + // Not using dogleg_markOutliers() (yet...) + +@@ -6810,7 +6813,6 @@ mrcal_optimize( // out + if(x_final) + memcpy(x_final, solver_context->beforeStep->x, ctx.Nmeasurements*sizeof(double)); + +- done: + if(solver_context != NULL) + dogleg_freeContext(&solver_context); + +@@ -6834,7 +6836,9 @@ bool mrcal_write_cameramodel_file(const char* filename, + { + MSG("Couldn't construct lensmodel string. Unconfigured string: '%s'", + mrcal_lensmodel_name_unconfigured(&cameramodel->lensmodel)); +- goto done; ++ if(fp != NULL) ++ fclose(fp); ++ return result; + } + + int Nparams = mrcal_lensmodel_num_params(&cameramodel->lensmodel); +@@ -6842,7 +6846,9 @@ bool mrcal_write_cameramodel_file(const char* filename, + { + MSG("Couldn't get valid Nparams from lensmodel string '%s'", + lensmodel_string); +- goto done; ++ if(fp != NULL) ++ fclose(fp); ++ return result;; + } + + fprintf(fp, "{\n"); +@@ -6865,7 +6871,6 @@ bool mrcal_write_cameramodel_file(const char* filename, + fprintf(fp,"}\n"); + result = true; + +- done: + if(fp != NULL) + fclose(fp); + return result; +diff --git a/poseutils.c b/poseutils.c +index 80d2b0d88dd5ad264bb579a7062b051d9972be74..59f7738477127829b87f490b5a2337c2378c2309 100644 +--- a/poseutils.c ++++ b/poseutils.c +@@ -1079,215 +1079,6 @@ void mrcal_r_from_R_full( // output + } + } + +-// LAPACK SVD function +-int dgesdd_(char* jobz, +- int* m, +- int* n, +- double* a, +- int* lda, +- double* s, +- double* u, +- int* ldu, +- double* vt, +- int* ldvt, +- double* work, +- int* lwork, +- int* iwork, +- int* info, +- int jobz_len); +- +-// This is functionally identical to mrcal.align_procrustes_vectors_R01(). It +-// should replace that function to provide a C implementation for mrcal users +-// +-// This solves: +-// https://en.wikipedia.org/wiki/Orthogonal_Procrustes_problem +-// See the mrcal sources for implementation details +-static +-bool _align_procrustes_vectors_R01(// out +- double* R01, +- // in +- const int N, +- // (N,3) arrays +- const double* p0, +- const double* p1, +- // (3,) array; may be NULL +- const double* pmean0, +- const double* pmean1, +- +- // (N,) array; may be NULL to use an even +- // weighting +- const double* weights) +-{ +- double M[9] = {}; +- +- double _pmean0[3] = {}; +- double _pmean1[3] = {}; +- if(pmean0 == NULL) pmean0 = _pmean0; +- if(pmean1 == NULL) pmean1 = _pmean1; +- +- if(weights == NULL) +- for(int i=0; i At = V Ut. And the results it gives back to me +- // are transposed too. So I give it At. The "U" it gives me back is actually +- // Vt and the Vt is actually U +- dgesdd_("A", +- (int[]){3}, (int[]){3}, +- M, (int[]){3}, +- S, +- Vt,(int[]){3}, +- U, (int[]){3}, +- &lwork_query, +- (int[]){-1}, // query the optimal lwork +- iwork, +- &info, +- 1); +- if(info != 0) +- { +- // secret value to indicate that this is a fatal error. Needed for the +- // Python layer +- R01[0] = 1.; +- return false; +- } +- +- double work[(int)lwork_query]; +- +- dgesdd_("A", +- (int[]){3}, (int[]){3}, +- M, (int[]){3}, +- S, +- Vt,(int[]){3}, +- U, (int[]){3}, +- work, +- (int[]){(int)lwork_query}, +- iwork, +- &info, +- 1); +- if(info != 0) +- { +- // secret value to indicate that this is a fatal error. Needed for the +- // Python layer +- R01[0] = 1.; +- return false; +- } +- +- // I look at the second-lowest singular value. One 0 singular value is OK +- // (the other two can uniquely define my 3D basis). But two isn't OK: the +- // basis is no longer unique +- if(S[1] < 1e-12) +- { +- // Poorly-defined problem +- // +- // secret value to indicate that this is a potentially non-fatal error. +- // Needed for the Python layer +- R01[0] = 0.; +- return false; +- } +- +- memset(R01, 0, 9*sizeof(R01[0])); +- for(int i=0; i<3; i++) +- for(int j=0; j<3; j++) +- for(int k=0; k<3; k++) +- // inner( U[i,:], V[j,:] +- R01[i*3 + j] += U[i*3 + k]*Vt[j + k*3]; +- +- // det(R01) is now +1 or -1. If it's -1, then this contains a mirror, and thus +- // is not a physical rotation. I compensate by negating the least-important +- // pair of singular vectors +- const double det_R = +- R01[0]*(R01[4]*R01[8]-R01[5]*R01[7]) - +- R01[1]*(R01[3]*R01[8]-R01[5]*R01[6]) + +- R01[2]*(R01[3]*R01[7]-R01[4]*R01[6]); +- if(det_R < 0) +- { +- memset(R01, 0, 9*sizeof(R01[0])); +- +- for(int i=0; i<3; i++) +- for(int j=0; j<3; j++) +- { +- int k; +- for(k=0; k<2; k++) +- R01[i*3 + j] += U[i*3 + k]*Vt[j + k*3]; +- R01[i*3 + j] -= U[i*3 + k]*Vt[j + k*3]; +- } +- } +- +- return true; +-} +- +-bool mrcal_align_procrustes_vectors_R01(// out +- double* R01, +- // in +- const int N, +- // (N,3) arrays +- const double* v0, +- const double* v1, +- +- // (N,) array; may be NULL to use an even +- // weighting +- const double* weights) +-{ +- return _align_procrustes_vectors_R01(R01,N,v0,v1,NULL,NULL,weights); +-} +- +-bool mrcal_align_procrustes_points_Rt01(// out +- double* Rt01, +- // in +- const int N, +- // (N,3) arrays +- const double* p0, +- const double* p1, +- +- // (N,) array; may be NULL to use an even +- // weighting +- const double* weights) +-{ +- double pmean0[3] = {}; +- double pmean1[3] = {}; +- +- for(int i=0; i +Date: Wed, 4 Dec 2024 00:58:21 -0500 +Subject: [PATCH 7/8] Place all C headers in an extern "C" block + +--- + mrcal.cpp | 5 ++++- + 1 file changed, 4 insertions(+), 1 deletion(-) + +diff --git a/mrcal.cpp b/mrcal.cpp +index 1560cd106ff5bcfe49476f785aa8108119318b3a..d4a2415a5730a48671b2716960bc343cdbd01295 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -14,19 +14,22 @@ + #include + #include + ++extern "C" { + #include ++} + #include + #include + #include + #include + ++extern "C" { + #include "mrcal.h" + #include "minimath/minimath.h" + #include "cahvore.h" + #include "minimath/minimath-extra.h" + #include "util.h" + #include "scales.h" +- ++} + #define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0) + + #define restrict __restrict diff --git a/upstream_utils/mrcal_patches/0008-Fix-UB-in-mrcal.patch b/upstream_utils/mrcal_patches/0008-Fix-UB-in-mrcal.patch new file mode 100644 index 00000000000..0014ab88cfe --- /dev/null +++ b/upstream_utils/mrcal_patches/0008-Fix-UB-in-mrcal.patch @@ -0,0 +1,70 @@ +From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001 +From: Matt +Date: Thu, 5 Dec 2024 00:10:15 -0500 +Subject: [PATCH 8/8] Fix UB in mrcal + +--- + mrcal.cpp | 27 ++++++++++++++++----------- + 1 file changed, 16 insertions(+), 11 deletions(-) + +diff --git a/mrcal.cpp b/mrcal.cpp +index d4a2415a5730a48671b2716960bc343cdbd01295..0b7263f58164bfe9f42447741a08366262195f7c 100644 +--- a/mrcal.cpp ++++ b/mrcal.cpp +@@ -4582,7 +4582,6 @@ void optimizer_callback(// input state + // Construct the FULL intrinsics vector, based on either the + // optimization vector or the inputs, depending on what we're optimizing + double* intrinsics_here = &intrinsics_all[icam_intrinsics][0]; +- double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore]; + + int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, +@@ -4604,15 +4603,20 @@ void optimizer_callback(// input state + &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics], + Ncore*sizeof(double) ); + } +- if( ctx->problem_selections.do_optimize_intrinsics_distortions ) +- { +- for(int i = 0; iNintrinsics-Ncore; i++) +- distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION; ++ int nDistortion = ctx->Nintrinsics-Ncore; ++ if (nDistortion) { ++ double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore]; ++ if( ctx->problem_selections.do_optimize_intrinsics_distortions ) ++ { ++ for(int i = 0; iNintrinsics-Ncore; i++) ++ distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION; ++ } ++ else { ++ memcpy( distortions_here, ++ &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore], ++ (ctx->Nintrinsics-Ncore)*sizeof(double) ); ++ } + } +- else +- memcpy( distortions_here, +- &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore], +- (ctx->Nintrinsics-Ncore)*sizeof(double) ); + } + for(int icam_extrinsics=0; + icam_extrinsicsNcameras_extrinsics; +@@ -4699,6 +4703,7 @@ void optimizer_callback(// input state + + int splined_intrinsics_grad_irun = 0; + ++ bool camera_at_identity = icam_extrinsics < 0; + project(q_hypothesis.data(), + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? +@@ -4720,9 +4725,9 @@ void optimizer_callback(// input state + + // input + intrinsics_all[icam_intrinsics].data(), +- &camera_rt[icam_extrinsics], &frame_rt, ++ camera_at_identity ? NULL : &camera_rt[icam_extrinsics], &frame_rt, + ctx->calobject_warp == NULL ? NULL : &calobject_warp_local, +- icam_extrinsics < 0, ++ camera_at_identity, + &ctx->lensmodel, &ctx->precomputed, + ctx->calibration_object_spacing, + ctx->calibration_object_width_n, diff --git a/wpical/.styleguide b/wpical/.styleguide new file mode 100644 index 00000000000..afaf7a1075b --- /dev/null +++ b/wpical/.styleguide @@ -0,0 +1,36 @@ +cppHeaderFileInclude { + \.h$ + \.inc$ + \.inl$ +} + +cppSrcFileInclude { + \.cpp$ +} + +generatedFileExclude { + src/main/native/resources/ + src/main/native/thirdparty/ + src/main/native/win/wpical.ico + src/main/native/mac/wpical.icns +} + +repoRootNameOverride { + wpical +} + +includeOtherLibs { + ^GLFW + ^ceres/ + ^fmt/ + ^frc/ + ^gtest/ + ^imgui + ^implot\.h$ + ^mrcal_wrapper\.h$ + ^opencv2\.h$ + ^portable-file-dialogs\.h$ + ^tagpose\.h$ + ^wpi/ + ^wpigui +} diff --git a/wpical/CMakeLists.txt b/wpical/CMakeLists.txt new file mode 100644 index 00000000000..f07d52755ac --- /dev/null +++ b/wpical/CMakeLists.txt @@ -0,0 +1,129 @@ +project(wpical LANGUAGES C CXX Fortran) + +include(CompileWarnings) +include(GenResources) +include(LinkMacOSGUI) +include(AddTest) + +configure_file(src/main/generate/WPILibVersion.cpp.in WPILibVersion.cpp) +generate_resources(src/main/native/resources generated/main/cpp WPIcal wpical wpical_resources_src) + +file(GLOB wpical_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp) +file( + GLOB_RECURSE wpical_thirdparty_src + src/main/native/thirdparty/libdogleg/src/*.cpp + src/main/native/thirdparty/mrcal/src/*.c + src/main/native/thirdparty/mrcal/src/*.cpp + src/main/native/thirdparty/mrcal_java/src/*.cpp +) + +if(WIN32) + set(wpical_rc src/main/native/win/wpical.rc) +elseif(APPLE) + set(MACOSX_BUNDLE_ICON_FILE wpical.icns) + set(APP_ICON_MACOSX src/main/native/mac/wpical.icns) + set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources") +endif() + +add_executable( + wpical + ${wpical_src} + ${wpical_thirdparty_src} + ${wpical_resources_src} + ${wpical_rc} + ${APP_ICON_MACOSX} +) +wpilib_link_macos_gui(wpical) +wpilib_target_warnings(wpical) +target_include_directories( + wpical + PRIVATE + src/main/native/include + src/main/native/thirdparty/libdogleg/include + src/main/native/thirdparty/mrcal/include + src/main/native/thirdparty/mrcal_java/include +) + +if(MSVC) + set(compile_flags + /wd4047 + /wd4098 + /wd4267 + /wd4068 + /wd4101 + /wd4200 + /wd4576 + /wd4715 + ) +elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang") + set(compile_flags + -Wno-format-nonliteral + -Wno-unused-variable + -Wno-missing-field-initializers + -Wno-gnu-anonymous-struct + -Wno-c99-extensions + -Wno-zero-length-array + -Wno-nested-anon-types + -Wno-sign-compare + -Wno-unused-function + -Wno-missing-braces + -Wno-null-conversion + -Wno-unused-but-set-variable + ) +else() + set(compile_flags + -Wno-format-nonliteral + -Wno-unused-variable + -Wno-unused-function + -Wno-sign-compare + -Wno-missing-field-initializers + ) +endif() +target_compile_options(wpical PRIVATE ${compile_flags}) + +find_package(OpenCV REQUIRED) +find_package(Ceres CONFIG REQUIRED) +target_link_libraries( + wpical + apriltag + ${OpenCV_LIBS} + wpigui + wpiutil + Ceres::ceres +) + +if(WIN32) + set_target_properties(wpical PROPERTIES WIN32_EXECUTABLE YES) +elseif(APPLE) + set_target_properties(wpical PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "wpical") +endif() + +if(WITH_TESTS) + wpilib_add_test(wpical src/test/native/cpp) + wpilib_link_macos_gui(wpical_test) + target_sources(wpical_test PRIVATE ${wpical_src} ${wpical_thirdparty_src}) + target_compile_definitions(wpical_test PRIVATE RUNNING_WPICAL_TESTS) + + if(MSVC) + target_compile_options(wpical_test PRIVATE /utf-8) + endif() + + target_compile_options(wpical_test PRIVATE ${compile_flags}) + target_include_directories( + wpical_test + PRIVATE + src/main/native/include + src/main/native/thirdparty/libdogleg/include + src/main/native/thirdparty/mrcal/include + src/main/native/thirdparty/mrcal_java/include + ) + target_link_libraries( + wpical_test + googletest + apriltag + ${OpenCV_LIBS} + wpigui + wpiutil + Ceres::ceres + ) +endif() diff --git a/wpical/Info.plist b/wpical/Info.plist new file mode 100644 index 00000000000..473286a8aa9 --- /dev/null +++ b/wpical/Info.plist @@ -0,0 +1,32 @@ + + + + + CFBundleName + WPIcal + CFBundleExecutable + wpical + CFBundleDisplayName + WPIcal + CFBundleIdentifier + edu.wpi.first.tools.WPIcal + CFBundleIconFile + ov.icns + CFBundlePackageType + APPL + CFBundleSupportedPlatforms + + MacOSX + + CFBundleInfoDictionaryVersion + 6.0 + CFBundleShortVersionString + 2021 + CFBundleVersion + 2021 + LSMinimumSystemVersion + 10.11 + NSHighResolutionCapable + + + diff --git a/wpical/build.gradle b/wpical/build.gradle new file mode 100644 index 00000000000..eea250c9036 --- /dev/null +++ b/wpical/build.gradle @@ -0,0 +1,249 @@ +import org.gradle.internal.os.OperatingSystem + +if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64')) { + return; +} + +description = "Field Calibration Tool" + +apply plugin: 'c' +apply plugin: 'cpp' +apply plugin: 'visual-studio' +apply plugin: 'google-test-test-suite' +apply plugin: 'edu.wpi.first.NativeUtils' + +if (OperatingSystem.current().isWindows()) { + apply plugin: 'windows-resources' +} + +ext { + nativeName = 'wpical' + useCpp = true + sharedCvConfigs = [ + wpicalTest: []] + staticCvConfigs = [] +} + +apply from: "${rootDir}/shared/resources.gradle" +apply from: "${rootDir}/shared/config.gradle" + +def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in") +def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp") + +apply from: "${rootDir}/shared/ceres.gradle" +apply from: "${rootDir}/shared/opencv.gradle" + +task generateCppVersion() { + description = 'Generates the wpilib version class' + group = 'WPILib' + + outputs.file wpilibVersionFileOutput + inputs.file wpilibVersionFileInput + + if (wpilibVersioning.releaseMode) { + outputs.upToDateWhen { false } + } + + // We follow a simple set of checks to determine whether we should generate a new version file: + // 1. If the release type is not development, we generate a new version file + // 2. If there is no generated version number, we generate a new version file + // 3. If there is a generated build number, and the release type is development, then we will + // only generate if the publish task is run. + doLast { + def version = wpilibVersioning.version.get() + println "Writing version ${version} to $wpilibVersionFileOutput" + + if (wpilibVersionFileOutput.exists()) { + wpilibVersionFileOutput.delete() + } + def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version) + wpilibVersionFileOutput.write(read) + } +} + +gradle.taskGraph.addTaskExecutionGraphListener { graph -> + def willPublish = graph.hasTask(publish) + if (willPublish) { + generateCppVersion.outputs.upToDateWhen { false } + } +} + +def generateTask = createGenerateResourcesTask('main', 'WPIcal', 'wpical', project) + +project(':').libraryBuild.dependsOn build +tasks.withType(CppCompile) { + dependsOn generateTask + dependsOn generateCppVersion +} + +// Suppress warnings +nativeUtils.platformConfigs.each { + if (it.name.contains('windows')) { + it.cCompiler.args.add("/wd4047") + it.cCompiler.args.add("/wd4098") + it.cCompiler.args.add("/wd4267") + it.cppCompiler.args.add("/wd4068") + it.cppCompiler.args.add("/wd4101") + it.cppCompiler.args.add("/wd4200") + it.cppCompiler.args.add("/wd4576") + it.cppCompiler.args.add("/wd4715") + } else if (it.name.contains('osx')) { + it.cCompiler.args.add("-Wno-format-nonliteral") + it.cCompiler.args.remove("-pedantic") + it.cCompiler.args.add("-Wno-unused-variable") + it.cCompiler.args.add("-Wno-unused-function") + it.cCompiler.args.add("-Wno-sign-compare") + it.cppCompiler.args.add("-Wno-missing-field-initializers") + it.cppCompiler.args.remove("-pedantic") + it.cppCompiler.args.add("-Wno-unused-variable") + it.cppCompiler.args.add("-Wno-unused-function") + it.cppCompiler.args.add("-Wno-sign-compare") + it.cppCompiler.args.remove("-permissive") + it.cppCompiler.args.add("-fpermissive") + it.cppCompiler.args.add("-Wno-missing-braces") + it.cppCompiler.args.add("-Wno-null-conversion") + it.cppCompiler.args.add("-Wno-unused-but-set-variable") + } else { + it.cCompiler.args.add("-Wno-format-nonliteral") + it.cCompiler.args.remove("-pedantic") + it.cCompiler.args.add("-Wno-unused-variable") + it.cCompiler.args.add("-Wno-unused-function") + it.cCompiler.args.add("-Wno-sign-compare") + it.cppCompiler.args.add("-Wno-missing-field-initializers") + it.cppCompiler.args.remove("-pedantic") + it.cppCompiler.args.add("-Wno-unused-variable") + it.cppCompiler.args.add("-Wno-unused-function") + it.cppCompiler.args.add("-Wno-sign-compare") + it.cppCompiler.args.add("-Wno-deprecated-declarations") + it.cppCompiler.args.add("-Wno-deprecated-enum-enum-conversion") + it.cppCompiler.args.remove("-permissive") + it.cppCompiler.args.add("-fpermissive") + } +} +def testResources = "\"$rootDir/wpical/src/main/native/resources\"".replace("\\", "/") +model { + components { + "${nativeName}"(NativeExecutableSpec) { + baseName = 'wpical' + sources { + cpp { + source { + srcDirs 'src/main/native/cpp', 'src/main/native/thirdparty/libdogleg/src', 'src/main/native/thirdparty/mrcal/src', 'src/main/native/thirdparty/mrcal_java/src', "$buildDir/generated/main/cpp" + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/main/native/include', 'src/main/native/thirdparty/libdogleg/include', 'src/main/native/thirdparty/mrcal/generated', 'src/main/native/thirdparty/mrcal/include', 'src/main/native/thirdparty/mrcal/include/minimath', 'src/main/native/thirdparty/mrcal_java/include' + } + } + c { + source { + srcDirs 'src/main/native/thirdparty/mrcal/src' + include '**/*.c' + } + exportedHeaders { + srcDirs 'src/main/native/include', 'src/main/native/thirdparty/libdogleg/include', 'src/main/native/thirdparty/mrcal/generated', 'src/main/native/thirdparty/mrcal/include', 'src/main/native/thirdparty/mrcal/include/minimath' + } + } + if (OperatingSystem.current().isWindows()) { + rc.source { + srcDirs 'src/main/native/win' + include '*.rc' + } + } + } + binaries.all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name.startsWith("linuxarm")) { + it.buildable = false + return + } + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' + lib project: ':wpigui', library: 'wpigui', linkage: 'static' + lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static' + lib project: ':apriltag', library: 'apriltag', linkage: 'static' + nativeUtils.useRequiredLibrary(it, 'ceres') + nativeUtils.useRequiredLibrary(it, 'opencv_static') + it.cppCompiler.define 'GLOG_USE_GLOG_EXPORT' + if (it.targetPlatform.operatingSystem.isWindows()) { + it.cppCompiler.define('GLOG_DEPRECATED', '__declspec(deprecated)') + } else { + it.cppCompiler.define('GLOG_DEPRECATED', '[[deprecated]]') + } + if (it.targetPlatform.operatingSystem.isWindows()) { + // Comdlg32.lib is needed for GetSaveFileNameA + // dbghelp.lib is needed for glog functions + it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' << 'Comdlg32.lib' << 'dbghelp.lib' + } else if (it.targetPlatform.operatingSystem.isMacOsX()) { + it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' << '-framework' << 'Accelerate' + } else { + it.linker.args << '-lX11' << "-lgfortran" + if (it.targetPlatform.name.startsWith('linuxarm')) { + it.linker.args << '-lGL' + } + } + } + } + } + testSuites { + "${nativeName}Test"(GoogleTestTestSuiteSpec) { + for(NativeComponentSpec c : $.components) { + if (c.name == nativeName) { + testing c + break + } + } + sources { + cpp { + source { + srcDirs 'src/test/native/cpp' + include '**/*.cpp' + } + exportedHeaders { + srcDirs 'src/test/native/include', 'src/main/native/cpp' + } + } + } + binaries.all { + if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name.startsWith("linuxarm")) { + it.buildable = false + return + } + lib project: ':wpiutil', library: 'wpiutil', linkage: 'static' + lib project: ':wpigui', library: 'wpigui', linkage: 'static' + lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static' + lib project: ':apriltag', library: 'apriltag', linkage: 'static' + nativeUtils.useRequiredLibrary(it, 'ceres') + nativeUtils.useRequiredLibrary(it, 'opencv_static') + it.cppCompiler.define('PROJECT_ROOT_PATH', testResources) + it.cppCompiler.define 'GLOG_USE_GLOG_EXPORT' + if (it.targetPlatform.name == nativeUtils.wpi.platforms.windowsarm64) { + // https://developercommunity.visualstudio.com/t/-imp-std-init-once-complete-unresolved-external-sy/1684365#T-N10041864 + it.linker.args << '/alternatename:__imp___std_init_once_complete=__imp_InitOnceComplete' + it.linker.args << '/alternatename:__imp___std_init_once_begin_initialize=__imp_InitOnceBeginInitialize' + } + if (it.targetPlatform.operatingSystem.isWindows()) { + it.cppCompiler.define('GLOG_DEPRECATED', '__declspec(deprecated)') + } else { + it.cppCompiler.define('GLOG_DEPRECATED', '[[deprecated]]') + } + lib project: ':thirdparty:googletest', library: 'googletest', linkage: 'static' + it.cppCompiler.define("RUNNING_WPICAL_TESTS") + if (it.targetPlatform.operatingSystem.isWindows()) { + it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' + // Comdlg32.lib is needed for GetSaveFileNameA + // dbghelp.lib is needed for glog functions + // advapi32.lib is needed for __imp_RegDeleteKeyA and other __imp_Reg functions for icvLoadWindowPos and the save variant + it.linker.args << 'advapi32.lib' << 'Comdlg32.lib' << 'dbghelp.lib' + } else if (it.targetPlatform.operatingSystem.isMacOsX()) { + it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' << '-framework' << 'Accelerate' + } else { + it.linker.args << '-lX11' << '-lgfortran' + if (it.targetPlatform.name.startsWith('linuxarm')) { + it.linker.args << '-lGL' + } + } + } + } + } +} + +apply from: 'publish.gradle' diff --git a/wpical/generate_mrcal.py b/wpical/generate_mrcal.py new file mode 100755 index 00000000000..eacfabfda0a --- /dev/null +++ b/wpical/generate_mrcal.py @@ -0,0 +1,32 @@ +#!/usr/bin/env python3 +import argparse +import subprocess +import sys +from pathlib import Path + + +def main(argv): + script_path = Path(__file__).resolve() + dirname = script_path.parent + + parser = argparse.ArgumentParser() + parser.add_argument( + "--output_directory", + help="Optional. If set, will output the generated files to this directory, otherwise it will use a path relative to the script", + default=dirname / "src/main/native/thirdparty/mrcal/generated/", + type=Path, + ) + args = parser.parse_args(argv) + + args.output_directory.mkdir(parents=True, exist_ok=True) + result = subprocess.run( + f"{dirname}/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl", + capture_output=True, + ) + (args.output_directory / "minimath_generated.h").write_text( + str(result.stdout, encoding="UTF8") + ) + + +if __name__ == "__main__": + main(sys.argv[1:]) diff --git a/wpical/publish.gradle b/wpical/publish.gradle new file mode 100644 index 00000000000..d00f47aafb8 --- /dev/null +++ b/wpical/publish.gradle @@ -0,0 +1,124 @@ +apply plugin: 'maven-publish' + +def baseArtifactId = 'wpical' +def artifactGroupId = 'edu.wpi.first.tools' +def zipBaseName = '_GROUP_edu_wpi_first_tools_ID_wpical_CLS' + +def outputsFolder = file("$project.buildDir/outputs") + +model { + tasks { + // Create the run task. + $.components.wpical.binaries.each { bin -> + if (bin.buildable && bin.name.toLowerCase().contains("debug") && nativeUtils.isNativeDesktopPlatform(bin.targetPlatform)) { + Task run = project.tasks.create("run", Exec) { + commandLine bin.tasks.install.runScriptFile.get().asFile.toString() + } + run.dependsOn bin.tasks.install + } + } + } + publishing { + def wpicalTaskList = [] + $.components.each { component -> + component.binaries.each { binary -> + if (binary in NativeExecutableBinarySpec && binary.component.name.contains("wpical")) { + if (binary.buildable && (binary.name.contains('Release') || binary.name.contains('release'))) { + // We are now in the binary that we want. + // This is the default application path for the ZIP task. + def applicationPath = binary.executable.file + def icon = file("$project.projectDir/src/main/native/mac/wpical.icns") + + // Create the ZIP. + def task = project.tasks.create("copywpicalExecutable" + binary.targetPlatform.operatingSystem.name + binary.targetPlatform.architecture.name, Zip) { + description("Copies the wpical executable to the outputs directory.") + destinationDirectory = outputsFolder + + archiveBaseName = zipBaseName + duplicatesStrategy = 'exclude' + archiveClassifier = nativeUtils.getPublishClassifier(binary) + + from(licenseFile) { + into '/' + } + + if (binary.targetPlatform.operatingSystem.isWindows()) { + def exePath = binary.executable.file.absolutePath + exePath = exePath.substring(0, exePath.length() - 4) + def pdbPath = new File(exePath + '.pdb') + from(pdbPath) + } + + into(nativeUtils.getPlatformPath(binary)) + } + + if (binary.targetPlatform.operatingSystem.isMacOsX()) { + // Create the macOS bundle. + def bundleTask = project.tasks.create("bundlewpicalOsxApp" + binary.targetPlatform.architecture.name, Copy) { + description("Creates a macOS application bundle for wpical") + from(file("$project.projectDir/Info.plist")) + into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/wpical.app/Contents")) + into("MacOS") { + with copySpec { + from binary.executable.file + } + } + into("Resources") { + with copySpec { + from icon + } + } + + inputs.property "HasDeveloperId", project.hasProperty("developerID") + + doLast { + if (project.hasProperty("developerID")) { + // Get path to binary. + exec { + workingDir rootDir + def args = [ + "sh", + "-c", + "codesign --force --strict --deep " + + "--timestamp --options=runtime " + + "--verbose -s ${project.findProperty("developerID")} " + + "$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/wpical.app/" + ] + commandLine args + } + } + } + } + + // Reset the application path if we are creating a bundle. + applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name") + task.from(applicationPath) + project.build.dependsOn bundleTask + + bundleTask.dependsOn binary.tasks.link + task.dependsOn(bundleTask) + } else { + task.from(applicationPath) + } + + task.dependsOn binary.tasks.link + wpicalTaskList.add(task) + project.build.dependsOn task + project.artifacts { task } + addTaskToCopyAllOutputs(task) + } + } + } + } + + publications { + wpical(MavenPublication) { + wpicalTaskList.each { artifact it } + + artifactId = baseArtifactId + groupId = artifactGroupId + version wpilibVersioning.version.get() + } + } + } +} diff --git a/wpical/src/main/generate/WPILibVersion.cpp.in b/wpical/src/main/generate/WPILibVersion.cpp.in new file mode 100644 index 00000000000..cfe24411588 --- /dev/null +++ b/wpical/src/main/generate/WPILibVersion.cpp.in @@ -0,0 +1,7 @@ +/** + * Autogenerated file! Do not manually edit this file. This version is regenerated + * any time the publish task is run, or when this file is deleted. + */ +const char* GetWPILibVersion() { + return "${wpilib_version}"; +} diff --git a/wpical/src/main/native/cpp/WPIcal.cpp b/wpical/src/main/native/cpp/WPIcal.cpp new file mode 100644 index 00000000000..75066b51242 --- /dev/null +++ b/wpical/src/main/native/cpp/WPIcal.cpp @@ -0,0 +1,591 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +namespace gui = wpi::gui; + +const char* GetWPILibVersion(); + +#ifdef __linux__ +const bool showDebug = false; +#else +const bool showDebug = true; +#endif + +namespace wpical { +std::string_view GetResource_wpical_16_png(); +std::string_view GetResource_wpical_32_png(); +std::string_view GetResource_wpical_48_png(); +std::string_view GetResource_wpical_64_png(); +std::string_view GetResource_wpical_128_png(); +std::string_view GetResource_wpical_256_png(); +std::string_view GetResource_wpical_512_png(); +} // namespace wpical + +void drawCheck() { + ImGui::SameLine(); + ImDrawList* draw_list = ImGui::GetWindowDrawList(); + + ImVec2 pos = ImGui::GetCursorScreenPos(); + float size = ImGui::GetFontSize(); + + ImVec2 p1 = ImVec2(pos.x + size * 0.25f, pos.y + size * 0.5f); + ImVec2 p2 = ImVec2(pos.x + size * 0.45f, pos.y + size * 0.7f); + ImVec2 p3 = ImVec2(pos.x + size * 0.75f, pos.y + size * 0.3f); + + float thickness = 3.0f; + draw_list->AddLine(p1, p2, IM_COL32(0, 255, 0, 255), thickness); + draw_list->AddLine(p2, p3, IM_COL32(0, 255, 0, 255), thickness); + ImGui::NewLine(); +} + +static void DisplayGui() { + ImGui::GetStyle().WindowRounding = 0; + + // fill entire OS window with this window + ImGui::SetNextWindowPos(ImVec2(0, 0)); + int width, height; + glfwGetWindowSize(gui::GetSystemWindow(), &width, &height); + ImGui::SetNextWindowSize( + ImVec2(static_cast(width), static_cast(height))); + + ImGui::Begin("Entries", nullptr, + ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_MenuBar | + ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove | + ImGuiWindowFlags_NoCollapse); + + // main menu + ImGui::BeginMenuBar(); + gui::EmitViewMenu(); + if (ImGui::BeginMenu("View")) { + ImGui::EndMenu(); + } + ImGui::EndMenuBar(); + + static std::unique_ptr camera_intrinsics_selector; + static std::string selected_camera_intrinsics; + + static std::unique_ptr field_map_selector; + static std::string selected_field_map; + + static std::unique_ptr + field_calibration_directory_selector; + static std::string selected_field_calibration_directory; + + static std::unique_ptr download_directory_selector; + static std::string selected_download_directory; + + static std::string calibration_json_path; + + cameracalibration::CameraModel cameraModel = { + .intrinsic_matrix = Eigen::Matrix::Identity(), + .distortion_coefficients = Eigen::Matrix::Zero(), + .avg_reprojection_error = 0.0}; + static bool mrcal = true; + + static double squareWidth = 0.709; + static double markerWidth = 0.551; + static int boardWidth = 12; + static int boardHeight = 8; + static double imagerWidth = 1920; + static double imagerHeight = 1080; + + static int pinnedTag = 1; + + static int focusedTag = 1; + static int referenceTag = 1; + + static Fieldmap currentCalibrationMap; + static Fieldmap currentReferenceMap; + + // camera matrix selector button + if (ImGui::Button("Upload Camera Intrinsics")) { + camera_intrinsics_selector = std::make_unique( + "Select Camera Intrinsics JSON", "", + std::vector{"JSON", "*.json"}, pfd::opt::none); + } + + ImGui::SameLine(); + ImGui::Text("Or"); + ImGui::SameLine(); + + // camera calibration button + if (ImGui::Button("Calibrate Camera")) { + selected_camera_intrinsics.clear(); + ImGui::OpenPopup("Camera Calibration"); + } + + if (camera_intrinsics_selector) { + auto selectedFiles = camera_intrinsics_selector->result(); + if (!selectedFiles.empty()) { + selected_camera_intrinsics = selectedFiles[0]; + } + camera_intrinsics_selector.reset(); + } + + if (!selected_camera_intrinsics.empty()) { + drawCheck(); + } + + // field json selector button + if (ImGui::Button("Select Field Map JSON")) { + field_map_selector = std::make_unique( + "Select Json File", "", + std::vector{"JSON Files", "*.json"}, pfd::opt::none); + } + + if (field_map_selector) { + auto selectedFiles = field_map_selector->result(); + if (!selectedFiles.empty()) { + selected_field_map = selectedFiles[0]; + } + field_map_selector.reset(); + } + + if (!selected_field_map.empty()) { + drawCheck(); + } + + // field calibration directory selector button + if (ImGui::Button("Select Field Calibration Folder")) { + field_calibration_directory_selector = std::make_unique( + "Select Field Calibration Folder", ""); + } + + if (field_calibration_directory_selector) { + auto selectedFiles = field_calibration_directory_selector->result(); + if (!selectedFiles.empty()) { + selected_field_calibration_directory = selectedFiles; + } + field_calibration_directory_selector.reset(); + } + + if (!selected_field_calibration_directory.empty()) { + drawCheck(); + } + + // pinned tag text field + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Pinned Tag", &pinnedTag); + + if (pinnedTag < 1) { + pinnedTag = 1; + } else if (pinnedTag > 16) { + pinnedTag = 16; + } + + // calibrate button + if (ImGui::Button("Calibrate!!!")) { + if (!selected_field_calibration_directory.empty() && + !selected_camera_intrinsics.empty() && !selected_field_map.empty() && + pinnedTag > 0 && pinnedTag <= 16) { + download_directory_selector = + std::make_unique("Select Download Folder", ""); + if (download_directory_selector) { + auto selectedFiles = download_directory_selector->result(); + if (!selectedFiles.empty()) { + selected_download_directory = selectedFiles; + } + download_directory_selector.reset(); + } + + calibration_json_path = selected_download_directory + "/output.json"; + + int calibrationOutput = fieldcalibration::calibrate( + selected_field_calibration_directory.c_str(), calibration_json_path, + selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag, + showDebug); + + if (calibrationOutput == 1) { + ImGui::OpenPopup("Fmap Conversion failed"); + } else if (calibrationOutput == 0) { + std::ifstream caljsonpath(calibration_json_path); + try { + wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath)); + std::ofstream out(selected_download_directory + "/output.fmap"); + out << fmap.dump(4); + out.close(); + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Visualize Calibration"); + } catch (...) { + ImGui::OpenPopup("Field Calibration Error"); + } + } + } + } + if (ImGui::Button("Visualize")) { + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Visualize Calibration"); + } + if (selected_field_calibration_directory.empty() || + selected_camera_intrinsics.empty() || selected_field_map.empty()) { + ImGui::TextWrapped( + "Some inputs are empty! please enter your camera calibration video, " + "field map, and field calibration directory"); + } else if (!(pinnedTag > 0 && pinnedTag <= 16)) { + ImGui::TextWrapped("Make sure the pinned tag is a valid april tag (1-16)"); + } else { + ImGui::TextWrapped("Calibration Ready"); + } + + // error popup window + if (ImGui::BeginPopupModal("Field Calibration Error", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped( + "Field Calibration Failed - please try again, ensuring that:"); + ImGui::TextWrapped( + "- You have selected the correct camera intrinsics JSON or camera " + "calibration video"); + ImGui::TextWrapped("- You have selected the correct ideal field map JSON"); + ImGui::TextWrapped( + "- You have selected the correct field calibration video directory"); + ImGui::TextWrapped( + "- Your field calibration video directory contains only field " + "calibration videos and no other files"); + ImGui::TextWrapped("- Your pinned tag is a valid FRC Apriltag"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + if (ImGui::BeginPopupModal("Fmap Conversion failed", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped( + "Fmap conversion failed - you can still use the calibration output on " + "Limelight platforms by converting the .json output to .fmap using the " + "Limelight map builder tool"); + ImGui::TextWrapped("https://tools.limelightvision.io/map-builder"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + // successful field calibration popup window + if (ImGui::BeginPopupModal("Success", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped("Success, output JSON generated in selected directory"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + // camera calibration popup window + if (ImGui::BeginPopupModal("Camera Calibration", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + // Camera Calibration Error calibration popup window + if (ImGui::BeginPopupModal("Camera Calibration Error", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + ImGui::TextWrapped( + "Camera calibration failed. Please make sure you have uploaded the " + "correct video file"); + ImGui::Separator(); + if (ImGui::Button("OK", ImVec2(120, 0))) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + if (ImGui::Button("MRcal")) { + mrcal = true; + } + + ImGui::SameLine(); + ImGui::Text("or"); + ImGui::SameLine(); + + if (ImGui::Button("OpenCV")) { + mrcal = false; + } + + if (mrcal) { + if (ImGui::Button("Select Camera Calibration Video")) { + camera_intrinsics_selector = std::make_unique( + "Select Camera Calibration Video", "", + std::vector{"Video Files", + "*.mp4 *.mov *.m4v *.mkv *.avi"}, + pfd::opt::none); + } + + if (camera_intrinsics_selector) { + auto selectedFiles = camera_intrinsics_selector->result(); + if (!selectedFiles.empty()) { + selected_camera_intrinsics = selectedFiles[0]; + } + camera_intrinsics_selector.reset(); + } + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Square Width (in)", &squareWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Marker Width (in)", &markerWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Width (squares)", &boardWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Height (squares)", &boardHeight); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Image Width (pixels)", &imagerWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Image Height (pixels)", &imagerHeight); + + ImGui::Separator(); + if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) { + std::cout << "calibration button pressed" << std::endl; + int ret = cameracalibration::calibrate( + selected_camera_intrinsics.c_str(), cameraModel, markerWidth, + boardWidth, boardHeight, imagerWidth, imagerHeight, showDebug); + if (ret == 0) { + size_t lastSeparatorPos = + selected_camera_intrinsics.find_last_of("/\\"); + std::string output_file_path; + + if (lastSeparatorPos != std::string::npos) { + output_file_path = + selected_camera_intrinsics.substr(0, lastSeparatorPos) + .append("/cameracalibration.json"); + } + + selected_camera_intrinsics = output_file_path; + + cameracalibration::dumpJson(cameraModel, output_file_path); + ImGui::CloseCurrentPopup(); + } else if (ret == 1) { + std::cout << "calibration failed and popup ready" << std::endl; + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Camera Calibration Error"); + } else { + ImGui::CloseCurrentPopup(); + } + } + } else { + if (ImGui::Button("Select Camera Calibration Video")) { + camera_intrinsics_selector = std::make_unique( + "Select Camera Calibration Video", "", + std::vector{"Video Files", + "*.mp4 *.mov *.m4v *.mkv *.avi"}, + pfd::opt::none); + } + + if (camera_intrinsics_selector) { + auto selectedFiles = camera_intrinsics_selector->result(); + if (!selectedFiles.empty()) { + selected_camera_intrinsics = selectedFiles[0]; + } + camera_intrinsics_selector.reset(); + } + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Square Width (in)", &squareWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputDouble("Marker Width (in)", &markerWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Width (squares)", &boardWidth); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Board Height (squares)", &boardHeight); + + ImGui::Separator(); + if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) { + std::cout << "calibration button pressed" << std::endl; + int ret = cameracalibration::calibrate( + selected_camera_intrinsics.c_str(), cameraModel, squareWidth, + markerWidth, boardWidth, boardHeight, showDebug); + if (ret == 0) { + size_t lastSeparatorPos = + selected_camera_intrinsics.find_last_of("/\\"); + std::string output_file_path; + + if (lastSeparatorPos != std::string::npos) { + output_file_path = + selected_camera_intrinsics.substr(0, lastSeparatorPos) + .append("/cameracalibration.json"); + } + + selected_camera_intrinsics = output_file_path; + + cameracalibration::dumpJson(cameraModel, output_file_path); + ImGui::CloseCurrentPopup(); + } else if (ret == 1) { + std::cout << "calibration failed and popup ready" << std::endl; + ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always); + ImGui::OpenPopup("Camera Calibration Error"); + } else { + ImGui::CloseCurrentPopup(); + } + } + } + + ImGui::SameLine(); + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + + ImGui::EndPopup(); + } + + // visualize calibration popup + if (ImGui::BeginPopupModal("Visualize Calibration", NULL, + ImGuiWindowFlags_AlwaysAutoResize)) { + if (ImGui::Button("Load Calibrated Field")) { + calibration_json_path = + std::make_unique( + "Select Json File", "", + std::vector{"JSON Files", "*.json"}, pfd::opt::none) + ->result()[0]; + } + + if (!calibration_json_path.empty()) { + ImGui::SameLine(); + drawCheck(); + } + + if (ImGui::Button("Load Reference Field")) { + selected_field_map = + std::make_unique( + "Select Json File", "", + std::vector{"JSON Files", "*.json"}, pfd::opt::none) + ->result()[0]; + } + + if (!selected_field_map.empty()) { + ImGui::SameLine(); + drawCheck(); + } + + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Focused Tag", &focusedTag); + ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12); + ImGui::InputInt("Reference Tag", &referenceTag); + + if (focusedTag < 1) { + focusedTag = 1; + } else if (focusedTag > 16) { + focusedTag = 16; + } + + if (referenceTag < 1) { + referenceTag = 1; + } else if (referenceTag > 16) { + referenceTag = 16; + } + + if (!calibration_json_path.empty() && !selected_field_map.empty()) { + std::ifstream calJson(calibration_json_path); + std::ifstream refJson(selected_field_map); + + currentCalibrationMap = Fieldmap(wpi::json::parse(calJson)); + currentReferenceMap = Fieldmap(wpi::json::parse(refJson)); + + double xDiff = currentReferenceMap.getTag(focusedTag).xPos - + currentCalibrationMap.getTag(focusedTag).xPos; + double yDiff = currentReferenceMap.getTag(focusedTag).yPos - + currentCalibrationMap.getTag(focusedTag).yPos; + double zDiff = currentReferenceMap.getTag(focusedTag).zPos - + currentCalibrationMap.getTag(focusedTag).zPos; + double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot - + currentCalibrationMap.getTag(focusedTag).yawRot; + double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot - + currentCalibrationMap.getTag(focusedTag).pitchRot; + double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot - + currentCalibrationMap.getTag(focusedTag).rollRot; + + double xRef = currentCalibrationMap.getTag(referenceTag).xPos - + currentCalibrationMap.getTag(focusedTag).xPos; + double yRef = currentCalibrationMap.getTag(referenceTag).yPos - + currentCalibrationMap.getTag(focusedTag).yPos; + double zRef = currentCalibrationMap.getTag(referenceTag).zPos - + currentCalibrationMap.getTag(focusedTag).zPos; + + ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str()); + ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str()); + ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str()); + + ImGui::TextWrapped( + "Yaw Difference %s°", + std::to_string( + Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi))) + .c_str()); + ImGui::TextWrapped( + "Pitch Difference %s°", + std::to_string( + Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi))) + .c_str()); + ImGui::TextWrapped( + "Roll Difference %s°", + std::to_string( + Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi))) + .c_str()); + + ImGui::NewLine(); + + ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str()); + ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str()); + ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str()); + } + + if (ImGui::Button("Close")) { + ImGui::CloseCurrentPopup(); + } + ImGui::EndPopup(); + } + + ImGui::End(); +} + +#ifndef RUNNING_WPICAL_TESTS +#ifdef _WIN32 +int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine, + int nCmdShow) { + int argc = __argc; + char** argv = __argv; +#else +int main(int argc, char** argv) { +#endif + std::string_view saveDir; + if (argc == 2) { + saveDir = argv[1]; + } + + gui::CreateContext(); + + gui::AddIcon(wpical::GetResource_wpical_16_png()); + gui::AddIcon(wpical::GetResource_wpical_32_png()); + gui::AddIcon(wpical::GetResource_wpical_48_png()); + gui::AddIcon(wpical::GetResource_wpical_64_png()); + gui::AddIcon(wpical::GetResource_wpical_128_png()); + gui::AddIcon(wpical::GetResource_wpical_256_png()); + gui::AddIcon(wpical::GetResource_wpical_512_png()); + + gui::AddLateExecute(DisplayGui); + + gui::Initialize("wpical", 600, 400); + gui::Main(); + + gui::DestroyContext(); + + return 0; +} +#endif diff --git a/wpical/src/main/native/cpp/cameracalibration.cpp b/wpical/src/main/native/cpp/cameracalibration.cpp new file mode 100644 index 00000000000..270de38af92 --- /dev/null +++ b/wpical/src/main/native/cpp/cameracalibration.cpp @@ -0,0 +1,404 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "cameracalibration.h" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +static bool filter(std::vector charuco_corners, + std::vector charuco_ids, + std::vector> marker_corners, + std::vector marker_ids, int board_width, + int board_height) { + if (charuco_ids.empty() || charuco_corners.empty()) { + return false; + } + + if (charuco_corners.size() < 10 || charuco_ids.size() < 10) { + return false; + } + + for (int i = 0; i < charuco_ids.size(); i++) { + if (charuco_ids.at(i) > (board_width - 1) * (board_height - 1) - 1) { + return false; + } + } + + for (int i = 0; i < marker_ids.size(); i++) { + if (marker_ids.at(i) > ((board_width * board_height) / 2) - 1) { + return false; + } + } + + return true; +} + +int cameracalibration::calibrate(const std::string& input_video, + CameraModel& camera_model, float square_width, + float marker_width, int board_width, + int board_height, bool show_debug_window) { + // ChArUco Board + cv::aruco::Dictionary aruco_dict = + cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); + cv::Ptr charuco_board = new cv::aruco::CharucoBoard( + cv::Size(board_width, board_height), square_width * 0.0254, + marker_width * 0.0254, aruco_dict); + cv::aruco::CharucoDetector charuco_detector(*charuco_board); + + // Video capture + cv::VideoCapture video_capture(input_video); + cv::Size frame_shape; + + std::vector> all_charuco_corners; + std::vector> all_charuco_ids; + + std::vector> all_obj_points; + std::vector> all_img_points; + + while (video_capture.grab()) { + cv::Mat frame; + video_capture.retrieve(frame); + + cv::Mat debug_image = frame; + + if (frame.empty()) { + break; + } + + // Detect + cv::Mat frame_gray; + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + + frame_shape = frame_gray.size(); + + std::vector charuco_corners; + std::vector charuco_ids; + std::vector> marker_corners; + std::vector marker_ids; + + std::vector obj_points; + std::vector img_points; + + charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids, + marker_corners, marker_ids); + + if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids, + board_width, board_height)) { + continue; + } + + charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points, + img_points); + + all_charuco_corners.push_back(charuco_corners); + all_charuco_ids.push_back(charuco_ids); + + all_obj_points.push_back(obj_points); + all_img_points.push_back(img_points); + + if (show_debug_window) { + cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids); + cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners, + charuco_ids); + cv::imshow("Frame", debug_image); + if (cv::waitKey(1) == 'q') { + break; + } + } + } + + video_capture.release(); + if (show_debug_window) { + cv::destroyAllWindows(); + } + + // Calibrate + cv::Mat camera_matrix, dist_coeffs; + std::vector r_vecs, t_vecs; + std::vector std_dev_intrinsics, std_dev_extrinsics, per_view_errors; + double repError; + + try { + // see https://stackoverflow.com/a/75865177 + int flags = cv::CALIB_RATIONAL_MODEL | cv::CALIB_USE_LU; + repError = cv::calibrateCamera( + all_obj_points, all_img_points, frame_shape, camera_matrix, dist_coeffs, + r_vecs, t_vecs, cv::noArray(), cv::noArray(), cv::noArray(), flags); + } catch (...) { + std::cout << "calibration failed" << std::endl; + return 1; + } + + std::vector matrix = {camera_matrix.begin(), + camera_matrix.end()}; + + std::vector distortion = {dist_coeffs.begin(), + dist_coeffs.end()}; + + camera_model.intrinsic_matrix = Eigen::Matrix(matrix.data()); + camera_model.distortion_coefficients = + Eigen::Matrix(distortion.data()); + camera_model.avg_reprojection_error = repError; + return 0; +} + +int cameracalibration::calibrate(const std::string& input_video, + CameraModel& camera_model, float square_width, + float marker_width, int board_width, + int board_height, double imagerWidthPixels, + double imagerHeightPixels, + bool show_debug_window) { + // ChArUco Board + cv::aruco::Dictionary aruco_dict = + cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000); + cv::Ptr charuco_board = new cv::aruco::CharucoBoard( + cv::Size(board_width, board_height), square_width * 0.0254, + marker_width * 0.0254, aruco_dict); + cv::aruco::CharucoDetector charuco_detector(*charuco_board); + + // Video capture + cv::VideoCapture video_capture(input_video); + cv::Size frame_shape; + + // Detection output + std::vector observation_boards; + std::vector frames_rt_toref; + + cv::Size boardSize(board_width - 1, board_height - 1); + cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels); + + while (video_capture.grab()) { + cv::Mat frame; + video_capture.retrieve(frame); + + cv::Mat debug_image = frame; + + if (frame.empty()) { + break; + } + + // Detect + cv::Mat frame_gray; + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + + frame_shape = frame_gray.size(); + + std::vector charuco_corners; + std::vector charuco_ids; + std::vector> marker_corners; + std::vector marker_ids; + + std::vector obj_points; + std::vector img_points; + + std::vector points((board_width - 1) * (board_height - 1)); + + charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids, + marker_corners, marker_ids); + + if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids, + board_width, board_height)) { + continue; + } + + charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points, + img_points); + + for (int i = 0; i < charuco_ids.size(); i++) { + int id = charuco_ids.at(i); + points[id].x = img_points.at(i).x; + points[id].y = img_points.at(i).y; + points[id].z = 1.0f; + } + + for (int i = 0; i < points.size(); i++) { + if (points[i].z != 1.0f) { + points[i].x = -1.0f; + points[i].y = -1.0f; + points[i].z = -1.0f; + } + } + + frames_rt_toref.push_back( + getSeedPose(points.data(), boardSize, imagerSize, square_width, 1000)); + observation_boards.insert(observation_boards.end(), points.begin(), + points.end()); + + if (show_debug_window) { + cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids); + cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners, + charuco_ids); + cv::imshow("Frame", debug_image); + if (cv::waitKey(1) == 'q') { + break; + } + } + } + + video_capture.release(); + if (show_debug_window) { + cv::destroyAllWindows(); + } + + if (observation_boards.empty()) { + std::cout << "calibration failed" << std::endl; + return 1; + } else { + std::cout << "points detected" << std::endl; + } + + std::unique_ptr cal_result = + mrcal_main(observation_boards, frames_rt_toref, boardSize, + square_width * 0.0254, imagerSize, 1000); + + auto& stats = *cal_result; + + // Camera matrix and distortion coefficients + std::vector camera_matrix = { + // fx 0 cx + stats.intrinsics[0], 0, stats.intrinsics[2], + // 0 fy cy + 0, stats.intrinsics[1], stats.intrinsics[3], + // 0 0 1 + 0, 0, 1}; + + std::vector distortion_coefficients = {stats.intrinsics[4], + stats.intrinsics[5], + stats.intrinsics[6], + stats.intrinsics[7], + stats.intrinsics[8], + stats.intrinsics[9], + stats.intrinsics[10], + stats.intrinsics[11], + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0}; + + camera_model.intrinsic_matrix = + Eigen::Matrix(camera_matrix.data()); + camera_model.distortion_coefficients = + Eigen::Matrix(distortion_coefficients.data()); + camera_model.avg_reprojection_error = stats.rms_error; + + return 0; +} + +int cameracalibration::calibrate(const std::string& input_video, + CameraModel& camera_model, float square_width, + int board_width, int board_height, + double imagerWidthPixels, + double imagerHeightPixels, + bool show_debug_window) { + // Video capture + cv::VideoCapture video_capture(input_video); + + // Detection output + std::vector observation_boards; + std::vector frames_rt_toref; + + cv::Size boardSize(board_width - 1, board_height - 1); + cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels); + + while (video_capture.grab()) { + cv::Mat frame; + video_capture.retrieve(frame); + + if (frame.empty()) { + break; + } + + // Detect + cv::Mat frame_gray; + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + + std::vector corners; + + bool found = cv::findChessboardCorners( + frame_gray, boardSize, corners, + cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE); + + if (found) { + std::vector current_points; + for (int i = 0; i < corners.size(); i++) { + current_points.push_back( + mrcal_point3_t{{corners.at(i).x, corners.at(i).y, 1.0f}}); + } + frames_rt_toref.push_back(getSeedPose(current_points.data(), boardSize, + imagerSize, square_width * 0.0254, + 1000)); + observation_boards.insert(observation_boards.end(), + current_points.begin(), current_points.end()); + } + if (show_debug_window) { + cv::drawChessboardCorners(frame, boardSize, corners, found); + cv::imshow("Checkerboard Detection", frame); + if (cv::waitKey(30) == 'q') { + break; + } + } + } + + video_capture.release(); + if (show_debug_window) { + cv::destroyAllWindows(); + } + + if (observation_boards.empty()) { + std::cout << "calibration failed" << std::endl; + return 1; + } else { + std::cout << "points detected" << std::endl; + } + + std::unique_ptr cal_result = + mrcal_main(observation_boards, frames_rt_toref, boardSize, + square_width * 0.0254, imagerSize, 1000); + + auto& stats = *cal_result; + + // Camera matrix and distortion coefficients + std::vector camera_matrix = { + // fx 0 cx + stats.intrinsics[0], 0, stats.intrinsics[2], + // 0 fy cy + 0, stats.intrinsics[1], stats.intrinsics[3], + // 0 0 1 + 0, 0, 1}; + + std::vector distortion_coefficients = {stats.intrinsics[4], + stats.intrinsics[5], + stats.intrinsics[6], + stats.intrinsics[7], + stats.intrinsics[8], + stats.intrinsics[9], + stats.intrinsics[10], + stats.intrinsics[11], + 0.0, + 0.0, + 0.0, + 0.0, + 0.0, + 0.0}; + + // Save calibration output + camera_model.intrinsic_matrix = + Eigen::Matrix(camera_matrix.data()); + camera_model.distortion_coefficients = + Eigen::Matrix(distortion_coefficients.data()); + camera_model.avg_reprojection_error = stats.rms_error; + + return 0; +} diff --git a/wpical/src/main/native/cpp/fieldcalibration.cpp b/wpical/src/main/native/cpp/fieldcalibration.cpp new file mode 100644 index 00000000000..1f3ff6970a3 --- /dev/null +++ b/wpical/src/main/native/cpp/fieldcalibration.cpp @@ -0,0 +1,599 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "fieldcalibration.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "apriltag.h" +#include "tag36h11.h" + +struct Pose { + Eigen::Vector3d p; + Eigen::Quaterniond q; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +struct Constraint { + int id_begin; + int id_end; + Pose t_begin_end; + EIGEN_MAKE_ALIGNED_OPERATOR_NEW +}; + +class PoseGraphError { + public: + explicit PoseGraphError(Pose t_ab_observed) + : m_t_ab_observed(std::move(t_ab_observed)) {} + + template + bool operator()(const T* const p_a_ptr, const T* const q_a_ptr, + const T* const p_b_ptr, const T* const q_b_ptr, + T* residuals_ptr) const { + // Tag A + Eigen::Map> p_a(p_a_ptr); + Eigen::Map> q_a(q_a_ptr); + + // Tag B + Eigen::Map> p_b(p_b_ptr); + Eigen::Map> q_b(q_b_ptr); + + // Rotation between tag A to tag B + Eigen::Quaternion q_a_inverse = q_a.conjugate(); + Eigen::Quaternion q_ab_estimated = q_a_inverse * q_b; + + // Displacement between tag A and tag B in tag A's frame + Eigen::Matrix p_ab_estimated = q_a_inverse * (p_b - p_a); + + // Error between the orientations + Eigen::Quaternion delta_q = + m_t_ab_observed.q.template cast() * q_ab_estimated.conjugate(); + + // Residuals + Eigen::Map> residuals(residuals_ptr); + residuals.template block<3, 1>(0, 0) = + p_ab_estimated - m_t_ab_observed.p.template cast(); + residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec(); + + return true; + } + + static ceres::CostFunction* Create(const Pose& t_ab_observed) { + return new ceres::AutoDiffCostFunction( + new PoseGraphError(t_ab_observed)); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + const Pose m_t_ab_observed; +}; + +const double tagSizeMeters = 0.1651; + +inline cameracalibration::CameraModel load_camera_model(std::string path) { + Eigen::Matrix camera_matrix; + Eigen::Matrix camera_distortion; + + std::ifstream file(path); + + wpi::json json_data; + + try { + json_data = wpi::json::parse(file); + } catch (const wpi::json::parse_error& e) { + std::cout << e.what() << std::endl; + } + + bool isCalibdb = json_data.contains("camera"); + + if (!isCalibdb) { + for (int i = 0; i < camera_matrix.rows(); i++) { + for (int j = 0; j < camera_matrix.cols(); j++) { + camera_matrix(i, j) = + json_data["camera_matrix"][(i * camera_matrix.cols()) + j]; + } + } + + for (int i = 0; i < camera_distortion.rows(); i++) { + for (int j = 0; j < camera_distortion.cols(); j++) { + camera_distortion(i, j) = json_data["distortion_coefficients"] + [(i * camera_distortion.cols()) + j]; + } + } + } else { + for (int i = 0; i < camera_matrix.rows(); i++) { + for (int j = 0; j < camera_matrix.cols(); j++) { + try { + camera_matrix(i, j) = json_data["camera_matrix"][i][j]; + } catch (...) { + camera_matrix(i, j) = json_data["camera_matrix"]["data"] + [(i * camera_matrix.cols()) + j]; + } + } + } + + for (int i = 0; i < camera_distortion.rows(); i++) { + for (int j = 0; j < camera_distortion.cols(); j++) { + try { + camera_distortion(i, j) = + json_data["distortion_coefficients"] + [(i * camera_distortion.cols()) + j]; + } catch (...) { + camera_distortion(i, j) = 0.0; + } + } + } + } + + cameracalibration::CameraModel camera_model{ + camera_matrix, camera_distortion, json_data["avg_reprojection_error"]}; + return camera_model; +} + +inline cameracalibration::CameraModel load_camera_model(wpi::json json_data) { + // Camera matrix + Eigen::Matrix camera_matrix; + + for (int i = 0; i < camera_matrix.rows(); i++) { + for (int j = 0; j < camera_matrix.cols(); j++) { + camera_matrix(i, j) = + json_data["camera_matrix"][(i * camera_matrix.cols()) + j]; + } + } + + // Distortion coefficients + Eigen::Matrix camera_distortion; + + for (int i = 0; i < camera_distortion.rows(); i++) { + for (int j = 0; j < camera_distortion.cols(); j++) { + camera_distortion(i, j) = json_data["distortion_coefficients"] + [(i * camera_distortion.cols()) + j]; + } + } + + cameracalibration::CameraModel camera_model{ + camera_matrix, camera_distortion, json_data["avg_reprojection_error"]}; + return camera_model; +} + +inline std::map load_ideal_map(std::string path) { + std::ifstream file(path); + wpi::json json_data = wpi::json::parse(file); + std::map ideal_map; + + for (const auto& element : json_data["tags"]) { + ideal_map[element["ID"]] = element; + } + + return ideal_map; +} + +Eigen::Matrix get_tag_transform( + std::map& ideal_map, int tag_id) { + Eigen::Matrix transform = + Eigen::Matrix::Identity(); + + transform.block<3, 3>(0, 0) = + Eigen::Quaternion( + ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["W"], + ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["X"], + ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"], + ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"]) + .toRotationMatrix(); + + transform(0, 3) = ideal_map[tag_id]["pose"]["translation"]["x"]; + transform(1, 3) = ideal_map[tag_id]["pose"]["translation"]["y"]; + transform(2, 3) = ideal_map[tag_id]["pose"]["translation"]["z"]; + + return transform; +} + +inline Eigen::Matrix estimate_tag_pose( + apriltag_detection_t* tag_detection, + const Eigen::Matrix& camera_matrix, + const Eigen::Matrix& camera_distortion, double tag_size) { + cv::Mat camera_matrix_cv; + cv::Mat camera_distortion_cv; + + cv::eigen2cv(camera_matrix, camera_matrix_cv); + cv::eigen2cv(camera_distortion, camera_distortion_cv); + + std::vector points_2d = { + cv::Point2f(tag_detection->p[0][0], tag_detection->p[0][1]), + cv::Point2f(tag_detection->p[1][0], tag_detection->p[1][1]), + cv::Point2f(tag_detection->p[2][0], tag_detection->p[2][1]), + cv::Point2f(tag_detection->p[3][0], tag_detection->p[3][1])}; + + std::vector points_3d_box_base = { + cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0), + cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0), + cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0), + cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)}; + + std::vector r_vec; + std::vector t_vec; + + cv::solvePnP(points_3d_box_base, points_2d, camera_matrix_cv, + camera_distortion_cv, r_vec, t_vec, false, cv::SOLVEPNP_SQPNP); + + cv::Mat r_mat; + cv::Rodrigues(r_vec, r_mat); + + Eigen::Matrix camera_to_tag{ + {r_mat.at(0, 0), r_mat.at(0, 1), r_mat.at(0, 2), + t_vec.at(0)}, + {r_mat.at(1, 0), r_mat.at(1, 1), r_mat.at(1, 2), + t_vec.at(1)}, + {r_mat.at(2, 0), r_mat.at(2, 1), r_mat.at(2, 2), + t_vec.at(2)}, + {0.0, 0.0, 0.0, 1.0}}; + + return camera_to_tag; +} + +inline void draw_tag_cube(cv::Mat& frame, + Eigen::Matrix camera_to_tag, + const Eigen::Matrix& camera_matrix, + const Eigen::Matrix& camera_distortion, + double tag_size) { + cv::Mat camera_matrix_cv; + cv::Mat camera_distortion_cv; + + cv::eigen2cv(camera_matrix, camera_matrix_cv); + cv::eigen2cv(camera_distortion, camera_distortion_cv); + + std::vector points_3d_box_base = { + cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0), + cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0), + cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0), + cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)}; + + std::vector points_3d_box_top = { + cv::Point3f(-tag_size / 2.0, tag_size / 2.0, -tag_size), + cv::Point3f(tag_size / 2.0, tag_size / 2.0, -tag_size), + cv::Point3f(tag_size / 2.0, -tag_size / 2.0, -tag_size), + cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, -tag_size)}; + + std::vector points_2d_box_base = { + cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), + cv::Point2f(0.0, 0.0)}; + + std::vector points_2d_box_top = { + cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), + cv::Point2f(0.0, 0.0)}; + + Eigen::Matrix r_vec = camera_to_tag.block<3, 3>(0, 0); + Eigen::Matrix t_vec = camera_to_tag.block<3, 1>(0, 3); + + cv::Mat r_vec_cv; + cv::Mat t_vec_cv; + + cv::eigen2cv(r_vec, r_vec_cv); + cv::eigen2cv(t_vec, t_vec_cv); + + cv::projectPoints(points_3d_box_base, r_vec_cv, t_vec_cv, camera_matrix_cv, + camera_distortion_cv, points_2d_box_base); + cv::projectPoints(points_3d_box_top, r_vec_cv, t_vec_cv, camera_matrix_cv, + camera_distortion_cv, points_2d_box_top); + + for (int i = 0; i < 4; i++) { + cv::Point2f& point_base = points_2d_box_base.at(i); + cv::Point2f& point_top = points_2d_box_top.at(i); + + cv::line(frame, point_base, point_top, cv::Scalar(0, 255, 255), 5); + + int i_next = (i + 1) % 4; + cv::Point2f& point_base_next = points_2d_box_base.at(i_next); + cv::Point2f& point_top_next = points_2d_box_top.at(i_next); + + cv::line(frame, point_base, point_base_next, cv::Scalar(0, 255, 255), 5); + cv::line(frame, point_top, point_top_next, cv::Scalar(0, 255, 255), 5); + } +} + +inline bool process_video_file( + apriltag_detector_t* tag_detector, + const Eigen::Matrix& camera_matrix, + const Eigen::Matrix& camera_distortion, double tag_size, + const std::string& path, + std::map, + Eigen::aligned_allocator>>& poses, + std::vector>& constraints, + bool show_debug_window) { + if (show_debug_window) { + cv::namedWindow("Processing Frame", cv::WINDOW_NORMAL); + } + cv::VideoCapture video_input(path); + + if (!video_input.isOpened()) { + std::cout << "Unable to open video " << path << std::endl; + return false; + } + + cv::Mat frame; + cv::Mat frame_gray; + cv::Mat frame_debug; + + int frame_num = 0; + + while (video_input.read(frame)) { + std::cout << "Processing " << path << " - Frame " << frame_num++ + << std::endl; + + // Convert color frame to grayscale frame + cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY); + + // Clone color frame for debugging + frame_debug = frame.clone(); + + // Detect tags + image_u8_t tag_image = {frame_gray.cols, frame_gray.rows, frame_gray.cols, + frame_gray.data}; + zarray_t* tag_detections = + apriltag_detector_detect(tag_detector, &tag_image); + + // Skip this loop if there are no tags detected + if (zarray_size(tag_detections) == 0) { + std::cout << "No tags detected" << std::endl; + continue; + } + + // Find detection with the smallest tag ID + apriltag_detection_t* tag_detection_min = nullptr; + zarray_get(tag_detections, 0, &tag_detection_min); + + for (int i = 0; i < zarray_size(tag_detections); i++) { + apriltag_detection_t* tag_detection_i; + zarray_get(tag_detections, i, &tag_detection_i); + + if (tag_detection_i->id < tag_detection_min->id) { + tag_detection_min = tag_detection_i; + } + } + + Eigen::Matrix camera_to_tag_min = estimate_tag_pose( + tag_detection_min, camera_matrix, camera_distortion, tag_size); + + // Find transformation from smallest tag ID + for (int i = 0; i < zarray_size(tag_detections); i++) { + apriltag_detection_t* tag_detection_i; + zarray_get(tag_detections, i, &tag_detection_i); + + // Add tag ID to poses + if (poses.find(tag_detection_i->id) == poses.end()) { + poses[tag_detection_i->id] = {Eigen::Vector3d(0.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)}; + } + + // Estimate camera to tag pose + Eigen::Matrix caamera_to_tag = estimate_tag_pose( + tag_detection_i, camera_matrix, camera_distortion, tag_size); + + // Draw debug cube + if (show_debug_window) { + draw_tag_cube(frame_debug, caamera_to_tag, camera_matrix, + camera_distortion, tag_size); + } + + // Skip finding transformation from smallest tag ID to itself + if (tag_detection_i->id == tag_detection_min->id) { + continue; + } + + Eigen::Matrix tag_min_to_tag = + camera_to_tag_min.inverse() * caamera_to_tag; + + // Constraint + Constraint constraint; + constraint.id_begin = tag_detection_min->id; + constraint.id_end = tag_detection_i->id; + constraint.t_begin_end.p = tag_min_to_tag.block<3, 1>(0, 3); + constraint.t_begin_end.q = + Eigen::Quaterniond(tag_min_to_tag.block<3, 3>(0, 0)); + + constraints.push_back(constraint); + } + + apriltag_detections_destroy(tag_detections); + + // Show debug + if (show_debug_window) { + cv::imshow("Processing Frame", frame_debug); + cv::waitKey(1); + } + } + + video_input.release(); + if (show_debug_window) { + cv::destroyAllWindows(); + } + + return true; +} + +int fieldcalibration::calibrate(std::string input_dir_path, + std::string output_file_path, + std::string camera_model_path, + std::string ideal_map_path, int pinned_tag_id, + bool show_debug_window) { + // Silence OpenCV logging + cv::utils::logging::setLogLevel( + cv::utils::logging::LogLevel::LOG_LEVEL_SILENT); + + // Load camera model + Eigen::Matrix3d camera_matrix; + Eigen::Matrix camera_distortion; + + try { + auto camera_model = load_camera_model(camera_model_path); + camera_matrix = camera_model.intrinsic_matrix; + camera_distortion = camera_model.distortion_coefficients; + } catch (...) { + return 1; + } + + wpi::json json = wpi::json::parse(std::ifstream(ideal_map_path)); + if (!json.contains("tags")) { + return 1; + } + + // Load ideal field map + std::map ideal_map; + try { + ideal_map = load_ideal_map(ideal_map_path); + } catch (...) { + return 1; + } + + // Apriltag detector + apriltag_detector_t* tag_detector = apriltag_detector_create(); + tag_detector->nthreads = 8; + + apriltag_family_t* tag_family = tag36h11_create(); + apriltag_detector_add_family(tag_detector, tag_family); + + // Find tag poses + std::map, + Eigen::aligned_allocator>> + poses; + std::vector> constraints; + + for (const auto& entry : + std::filesystem::directory_iterator(input_dir_path)) { + if (entry.path().filename().string()[0] == '.') { + continue; + } + + const std::string path = entry.path().string(); + + bool success = process_video_file(tag_detector, camera_matrix, + camera_distortion, tagSizeMeters, path, + poses, constraints, show_debug_window); + + if (!success) { + std::cout << "Unable to process video " << path << std::endl; + return 1; + } + } + + // Build optimization problem + ceres::Problem problem; + ceres::Manifold* quaternion_manifold = new ceres::EigenQuaternionManifold; + + for (const auto& constraint : constraints) { + auto pose_begin_iter = poses.find(constraint.id_begin); + auto pose_end_iter = poses.find(constraint.id_end); + + ceres::CostFunction* cost_function = + PoseGraphError::Create(constraint.t_begin_end); + + problem.AddResidualBlock(cost_function, nullptr, + pose_begin_iter->second.p.data(), + pose_begin_iter->second.q.coeffs().data(), + pose_end_iter->second.p.data(), + pose_end_iter->second.q.coeffs().data()); + + problem.SetManifold(pose_begin_iter->second.q.coeffs().data(), + quaternion_manifold); + problem.SetManifold(pose_end_iter->second.q.coeffs().data(), + quaternion_manifold); + } + + // Pin tag + auto pinned_tag_iter = poses.find(pinned_tag_id); + if (pinned_tag_iter != poses.end()) { + problem.SetParameterBlockConstant(pinned_tag_iter->second.p.data()); + problem.SetParameterBlockConstant( + pinned_tag_iter->second.q.coeffs().data()); + } + + // Solve + ceres::Solver::Options options; + options.max_num_iterations = 200; + options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY; + options.num_threads = 10; + + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + std::cout << summary.BriefReport() << std::endl; + + // Output + std::map observed_map = ideal_map; + + Eigen::Matrix correction_a; + correction_a << 0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1; + + Eigen::Matrix correction_b; + correction_b << 0, 1, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 1; + + Eigen::Matrix pinned_tag_transform = + get_tag_transform(ideal_map, pinned_tag_id); + + for (const auto& [tag_id, pose] : poses) { + // Transformation from pinned tag + Eigen::Matrix transform = + Eigen::Matrix::Identity(); + + transform.block<3, 3>(0, 0) = pose.q.toRotationMatrix(); + transform.block<3, 1>(0, 3) = pose.p; + + // Transformation from world + Eigen::Matrix corrected_transform = + pinned_tag_transform * correction_a * transform * correction_b; + Eigen::Quaternion corrected_transform_q( + corrected_transform.block<3, 3>(0, 0)); + + observed_map[tag_id]["pose"]["translation"]["x"] = + corrected_transform(0, 3); + observed_map[tag_id]["pose"]["translation"]["y"] = + corrected_transform(1, 3); + observed_map[tag_id]["pose"]["translation"]["z"] = + corrected_transform(2, 3); + + observed_map[tag_id]["pose"]["rotation"]["quaternion"]["X"] = + corrected_transform_q.x(); + observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"] = + corrected_transform_q.y(); + observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"] = + corrected_transform_q.z(); + observed_map[tag_id]["pose"]["rotation"]["quaternion"]["W"] = + corrected_transform_q.w(); + } + + wpi::json observed_map_json; + + for (const auto& [tag_id, tag_json] : observed_map) { + observed_map_json["tags"].push_back(tag_json); + } + + observed_map_json["field"] = { + {"length", static_cast(json.at("field").at("length"))}, + {"width", static_cast(json.at("field").at("width"))}}; + + std::ofstream output_file(output_file_path); + output_file << observed_map_json.dump(4) << std::endl; + + return 0; +} diff --git a/wpical/src/main/native/cpp/fmap.cpp b/wpical/src/main/native/cpp/fmap.cpp new file mode 100644 index 00000000000..273c9ebe5fa --- /dev/null +++ b/wpical/src/main/native/cpp/fmap.cpp @@ -0,0 +1,40 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include "fmap.h" + +#include +#include + +wpi::json fmap::singleTag(int tag, const tag::Pose& tagpose) { + std::vector transform = {}; + for (int i = 0; i < 4; i++) { + for (int j = 0; j < 4; j++) { + transform.push_back(tagpose.transformMatrixFmap(i, j)); + } + } + + return {{"family", "apriltag3_36h11_classic"}, + {"id", tag}, + {"size", 165.1}, + {"transform", transform}, + {"unique", true}}; +} + +wpi::json fmap::convertfmap(const wpi::json& json) { + std::string fmapstart = "{\"fiducials\":["; + + std::string fmapend = "],\"type\":\"frc\"}"; + + Fieldmap fieldmap(json); + + for (int i = 0; i < fieldmap.getNumTags(); i++) { + fmapstart += singleTag(i + 1, fieldmap.getTag(i + 1)).dump(); + if (i != fieldmap.getNumTags() - 1) { + fmapstart += ","; + } + } + + return wpi::json::parse(fmapstart.append(fmapend)); +} diff --git a/wpical/src/main/native/cpp/tagpose.cpp b/wpical/src/main/native/cpp/tagpose.cpp new file mode 100644 index 00000000000..761bf89573e --- /dev/null +++ b/wpical/src/main/native/cpp/tagpose.cpp @@ -0,0 +1,29 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +namespace tag { +Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y, + double z, double field_length_meters, double field_width_meters) { + xPos = xpos; + yPos = ypos; + zPos = zpos; + quaternion = Eigen::Quaterniond(w, x, y, z); + rotationMatrix = quaternion.toRotationMatrix(); + transformMatrixFmap.setZero(); + transformMatrixFmap.block<3, 3>(0, 0) = rotationMatrix; + transformMatrixFmap(0, 3) = xpos - (field_length_meters / 2.0); + transformMatrixFmap(1, 3) = ypos - (field_width_meters / 2.0); + transformMatrixFmap(2, 3) = zpos; + transformMatrixFmap(3, 3) = 1; + transformMatrixFmap(3, 0) = 0; + transformMatrixFmap(3, 1) = 0; + transformMatrixFmap(3, 2) = 0; + Eigen::Vector3d eulerAngles = rotationMatrix.eulerAngles(0, 1, 2); + rollRot = eulerAngles[0]; + pitchRot = eulerAngles[1]; + yawRot = eulerAngles[2]; +} +} // namespace tag diff --git a/wpical/src/main/native/include/cameracalibration.h b/wpical/src/main/native/include/cameracalibration.h new file mode 100644 index 00000000000..bd2744d565b --- /dev/null +++ b/wpical/src/main/native/include/cameracalibration.h @@ -0,0 +1,51 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include +#include + +#include +#include + +namespace cameracalibration { +struct CameraModel { + Eigen::Matrix intrinsic_matrix; + Eigen::Matrix distortion_coefficients; + double avg_reprojection_error; +}; + +int calibrate(const std::string& input_video, CameraModel& camera_model, + float square_width, float marker_width, int board_width, + int board_height, bool show_debug_window); +int calibrate(const std::string& input_video, CameraModel& camera_model, + float square_width, float marker_width, int board_width, + int board_height, double imagerWidthPixels, + double imagerHeightPixels, bool show_debug_window); +int calibrate(const std::string& input_video, CameraModel& camera_model, + float square_width, int board_width, int board_height, + double imagerWidthPixels, double imagerHeightPixels, + bool show_debug_window); +static void dumpJson(CameraModel& camera_model, + const std::string& output_file_path) { + std::vector camera_matrix(camera_model.intrinsic_matrix.data(), + camera_model.intrinsic_matrix.data() + + camera_model.intrinsic_matrix.size()); + std::vector distortion_coefficients( + camera_model.distortion_coefficients.data(), + camera_model.distortion_coefficients.data() + + camera_model.distortion_coefficients.size()); + + wpi::json result = { + {"camera_matrix", camera_matrix}, + {"distortion_coefficients", distortion_coefficients}, + {"avg_reprojection_error", camera_model.avg_reprojection_error}}; + + std::ofstream output_file(output_file_path); + output_file << result.dump(4) << std::endl; + output_file.close(); +} +} // namespace cameracalibration diff --git a/wpical/src/main/native/include/fieldcalibration.h b/wpical/src/main/native/include/fieldcalibration.h new file mode 100644 index 00000000000..d9de45990ba --- /dev/null +++ b/wpical/src/main/native/include/fieldcalibration.h @@ -0,0 +1,15 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include "cameracalibration.h" + +namespace fieldcalibration { +int calibrate(std::string input_dir_path, std::string output_file_path, + std::string camera_model_path, std::string ideal_map_path, + int pinned_tag_id, bool show_debug_window); +} // namespace fieldcalibration diff --git a/wpical/src/main/native/include/fieldmap.h b/wpical/src/main/native/include/fieldmap.h new file mode 100644 index 00000000000..22fefc28a44 --- /dev/null +++ b/wpical/src/main/native/include/fieldmap.h @@ -0,0 +1,59 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +#include +#include + +class Fieldmap { + public: + Fieldmap() = default; + explicit Fieldmap(const wpi::json& json) { + double field_length_meters = + static_cast(json.at("field").at("length")); + double field_width_meters = + static_cast(json.at("field").at("width")); + for (const auto& tag : json.at("tags").items()) { + double tagXPos = + static_cast(tag.value().at("pose").at("translation").at("x")); + double tagYPos = + static_cast(tag.value().at("pose").at("translation").at("y")); + double tagZPos = + static_cast(tag.value().at("pose").at("translation").at("z")); + double tagWQuat = static_cast( + tag.value().at("pose").at("rotation").at("quaternion").at("W")); + double tagXQuat = static_cast( + tag.value().at("pose").at("rotation").at("quaternion").at("X")); + double tagYQuat = static_cast( + tag.value().at("pose").at("rotation").at("quaternion").at("Y")); + double tagZQuat = static_cast( + tag.value().at("pose").at("rotation").at("quaternion").at("Z")); + + tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat, + tagYQuat, tagZQuat, field_length_meters, + field_width_meters); + } + } + + const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; } + + int getNumTags() const { return tagVec.size(); } + + static double minimizeAngle(double angle) { + angle = std::fmod(angle, 360); + if (angle > 180) { + return angle - 360; + } else if (angle < -180) { + return angle + 360; + } + return angle; + } + + private: + std::vector tagVec; +}; diff --git a/wpical/src/main/native/include/fmap.h b/wpical/src/main/native/include/fmap.h new file mode 100644 index 00000000000..6757b7cada9 --- /dev/null +++ b/wpical/src/main/native/include/fmap.h @@ -0,0 +1,15 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include + +#include +#include + +namespace fmap { +wpi::json singleTag(int tag, const tag::Pose& tagpose); +wpi::json convertfmap(const wpi::json& json); +} // namespace fmap diff --git a/wpical/src/main/native/include/tagpose.h b/wpical/src/main/native/include/tagpose.h new file mode 100644 index 00000000000..422fc76a933 --- /dev/null +++ b/wpical/src/main/native/include/tagpose.h @@ -0,0 +1,20 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#pragma once + +#include +#include + +namespace tag { +class Pose { + public: + Pose(double xpos, double ypos, double zpos, double w, double x, double y, + double z, double field_length_meters, double field_width_meters); + double xPos, yPos, zPos, yawRot, rollRot, pitchRot; + Eigen::Quaterniond quaternion; + Eigen::Matrix3d rotationMatrix; + Eigen::Matrix4d transformMatrixFmap; +}; +} // namespace tag diff --git a/wpical/src/main/native/mac/wpical.icns b/wpical/src/main/native/mac/wpical.icns new file mode 100644 index 00000000000..06015595be1 Binary files /dev/null and b/wpical/src/main/native/mac/wpical.icns differ diff --git a/wpical/src/main/native/resources/2024-crescendo.json b/wpical/src/main/native/resources/2024-crescendo.json new file mode 100644 index 00000000000..8cb837a5023 --- /dev/null +++ b/wpical/src/main/native/resources/2024-crescendo.json @@ -0,0 +1,296 @@ +{ + "tags": [ + { + "ID": 1, + "pose": { + "translation": { + "x": 15.079471999999997, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 2, + "pose": { + "translation": { + "x": 16.185134, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 3, + "pose": { + "translation": { + "x": 16.579342, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 4, + "pose": { + "translation": { + "x": 16.579342, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 5, + "pose": { + "translation": { + "x": 14.700757999999999, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 6, + "pose": { + "translation": { + "x": 1.8415, + "y": 8.2042, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": -0.7071067811865475, + "X": -0.0, + "Y": 0.0, + "Z": 0.7071067811865476 + } + } + } + }, + { + "ID": 7, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 5.547867999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 8, + "pose": { + "translation": { + "x": -0.038099999999999995, + "y": 4.982717999999999, + "z": 1.4511020000000001 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 9, + "pose": { + "translation": { + "x": 0.356108, + "y": 0.883666, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 10, + "pose": { + "translation": { + "x": 1.4615159999999998, + "y": 0.24587199999999998, + "z": 1.355852 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 11, + "pose": { + "translation": { + "x": 11.904726, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.8660254037844387, + "X": -0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 12, + "pose": { + "translation": { + "x": 11.904726, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.8660254037844387, + "X": 0.0, + "Y": 0.0, + "Z": 0.49999999999999994 + } + } + } + }, + { + "ID": 13, + "pose": { + "translation": { + "x": 11.220196, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 6.123233995736766e-17, + "X": 0.0, + "Y": 0.0, + "Z": 1.0 + } + } + } + }, + { + "ID": 14, + "pose": { + "translation": { + "x": 5.320792, + "y": 4.105148, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 1.0, + "X": 0.0, + "Y": 0.0, + "Z": 0.0 + } + } + } + }, + { + "ID": 15, + "pose": { + "translation": { + "x": 4.641342, + "y": 4.49834, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": 0.5000000000000001, + "X": 0.0, + "Y": 0.0, + "Z": 0.8660254037844386 + } + } + } + }, + { + "ID": 16, + "pose": { + "translation": { + "x": 4.641342, + "y": 3.7132259999999997, + "z": 1.3208 + }, + "rotation": { + "quaternion": { + "W": -0.4999999999999998, + "X": -0.0, + "Y": 0.0, + "Z": 0.8660254037844387 + } + } + } + } + ], + "field": { + "length": 16.541, + "width": 8.211 + } +} diff --git a/wpical/src/main/native/resources/altfieldvideo/long.avi b/wpical/src/main/native/resources/altfieldvideo/long.avi new file mode 100644 index 00000000000..5df4dd35ce6 Binary files /dev/null and b/wpical/src/main/native/resources/altfieldvideo/long.avi differ diff --git a/wpical/src/main/native/resources/altfieldvideo/short.avi b/wpical/src/main/native/resources/altfieldvideo/short.avi new file mode 100644 index 00000000000..8a5d28e2f3f Binary files /dev/null and b/wpical/src/main/native/resources/altfieldvideo/short.avi differ diff --git a/wpical/src/main/native/resources/fieldvideo/long.mp4 b/wpical/src/main/native/resources/fieldvideo/long.mp4 new file mode 100644 index 00000000000..523ab0c869f Binary files /dev/null and b/wpical/src/main/native/resources/fieldvideo/long.mp4 differ diff --git a/wpical/src/main/native/resources/fieldvideo/short.mp4 b/wpical/src/main/native/resources/fieldvideo/short.mp4 new file mode 100644 index 00000000000..0d1643a5c54 Binary files /dev/null and b/wpical/src/main/native/resources/fieldvideo/short.mp4 differ diff --git a/wpical/src/main/native/resources/lifecam_1280p_10x10.vnl b/wpical/src/main/native/resources/lifecam_1280p_10x10.vnl new file mode 100644 index 00000000000..4db21a5862a --- /dev/null +++ b/wpical/src/main/native/resources/lifecam_1280p_10x10.vnl @@ -0,0 +1,1204 @@ +## generated with mrgingham --jobs 4 --gridn 10 /home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/*.png +# filename x y level +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 251.015437 296.974271 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.509411 297.531269 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.520930 297.109302 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.075949 297.800105 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 425.940824 297.621011 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 467.313812 298.319337 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 509.130579 298.103306 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 548.803849 298.629423 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 589.443478 298.758103 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 628.032861 299.023840 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 251.154750 340.004086 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.798148 339.659259 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.652445 339.811122 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.427900 339.400470 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 425.527248 339.720027 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 467.897135 338.868490 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 508.870179 339.166510 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 549.899648 338.867958 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 589.019359 339.137326 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 628.936450 338.489408 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 250.865169 383.082397 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.690674 382.433679 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.837000 382.513000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.269743 381.424135 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.337838 380.901138 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 467.986314 380.531478 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 509.646416 379.907655 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 549.761326 379.574498 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 590.184535 378.994802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 628.631792 378.915653 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 250.854308 426.266699 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.331000 425.828000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.680823 424.368350 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.214677 423.507119 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.182903 422.578032 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.320212 421.791824 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 509.632653 420.944341 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 550.398293 420.147937 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 589.803175 419.660317 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 629.710870 418.489855 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 250.721127 469.678873 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.393652 468.568925 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.620690 467.210345 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.173438 465.995755 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.304864 464.772242 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.503483 463.320398 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.123644 462.287419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 550.344845 461.222990 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 590.841542 459.922912 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 629.821138 459.304201 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 250.821820 512.940448 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.325482 511.647752 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.535069 510.021785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.162100 508.515982 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.202491 506.836492 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.304000 505.577000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.045658 503.621307 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 551.054721 502.112661 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 590.939336 500.918957 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 630.756393 499.445491 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 251.044590 556.417705 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.448980 554.871234 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.700613 553.239264 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.161168 551.239589 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.261317 549.263374 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.541840 547.375668 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.198598 545.350467 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 551.186118 543.658267 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 591.562349 541.892196 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 630.761721 540.378292 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 251.693449 599.738997 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 295.819931 598.087973 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 339.866451 596.038817 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.134593 593.874852 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.277143 591.888571 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 468.548673 589.714602 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.568214 587.343647 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 551.565421 585.352336 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 591.894858 583.191001 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 631.611511 581.210432 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 252.027027 642.986486 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 296.374491 641.043419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 340.162115 638.979663 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.585432 636.766187 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.407666 634.381533 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 469.002996 631.869196 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 510.772475 629.584906 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 551.706190 627.051874 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 592.329350 624.628827 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 632.000646 622.551969 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 252.861689 686.089963 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 296.932812 683.951220 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 340.908951 681.604938 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 383.883708 679.330773 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 426.932635 676.690487 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 469.262226 674.123946 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 511.052474 671.581209 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 552.267148 668.786101 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 592.656385 666.170265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img2.png 632.669465 663.506821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img9.png - - - +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 33.725373 205.770576 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 66.154079 184.195368 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 97.410349 163.550485 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 128.525066 142.348945 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 158.456728 122.444725 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 188.364369 102.185318 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 217.360747 82.825605 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 246.251193 63.495011 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 274.418169 44.810333 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 302.566722 26.268140 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 58.761305 235.412587 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 90.082251 213.942058 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 120.679173 192.735931 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 150.441873 172.412672 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 179.994505 151.795971 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 208.859013 132.026452 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 237.811258 112.350993 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 265.906844 93.376847 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 294.007736 74.433225 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 321.426879 55.879674 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 82.707277 264.083635 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 113.103559 242.659736 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 142.748059 221.957175 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 172.115163 201.362124 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 201.110328 180.915188 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 229.719728 160.984911 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 257.570457 141.494015 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 285.790958 122.426971 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 313.028417 103.270863 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 340.123120 84.679511 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 106.046612 292.229268 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 135.189369 270.904485 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 164.295097 249.963563 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 193.135522 229.392684 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 221.777822 209.050079 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 249.566908 189.174155 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 277.511446 169.474595 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 304.610937 150.516028 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 331.936703 131.619763 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 358.203623 113.219565 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 127.785714 319.432773 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 156.894940 298.478566 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 185.743784 277.343627 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 213.865377 256.787653 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 241.777546 236.643136 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 269.746660 216.476876 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 296.589336 197.439040 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 323.627969 178.178161 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 350.044477 159.458987 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 376.129792 140.651757 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 149.621249 345.784063 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 178.128072 324.861810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 206.406940 303.860673 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 234.122778 283.522286 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 261.916356 263.113479 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 288.846795 243.799359 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 315.758399 224.280861 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 341.913787 205.187239 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 367.880603 186.671559 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 393.244861 168.002418 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 170.640172 371.641894 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 198.878871 350.450412 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 226.583950 329.922880 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 254.119098 309.396406 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 281.142932 289.523822 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 307.732691 269.647239 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 334.136618 250.477044 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 360.002450 231.540427 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 385.484971 212.754881 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 410.349912 194.686165 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 191.637540 396.382802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 219.106787 375.773039 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 246.427054 355.399815 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 273.498347 334.846428 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 300.042506 314.870694 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 326.327887 295.231481 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 352.108108 275.892764 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 377.401973 257.078601 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 402.526776 238.664683 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 427.084655 220.048131 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 211.902338 421.048831 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 238.945342 400.331677 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 265.966763 379.784915 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 292.438931 359.509815 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 318.435282 339.733920 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 344.310945 319.956882 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 369.622007 300.966123 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 394.517337 281.905340 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 418.951189 263.451564 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 443.188438 245.154583 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 231.733915 444.557797 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 258.128422 424.035147 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 284.693299 403.438660 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 310.522819 383.378188 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 336.524351 363.578516 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 361.747836 344.118869 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 386.717939 325.015043 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 411.235345 306.066379 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 435.076995 287.761565 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img13.png 458.851006 269.592985 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img10.png - - - +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 606.166809 46.390932 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 656.527620 46.850567 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 706.010458 47.189542 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 757.207534 47.724947 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 807.846749 48.552632 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 859.281225 49.107977 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 911.102866 49.145400 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 963.236636 50.190456 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1014.831401 51.401449 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1067.427916 52.475098 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 605.264291 96.454343 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 654.468647 96.831683 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 705.278589 97.519465 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 755.314832 98.377616 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 806.522032 98.916157 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 857.656762 99.932377 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 909.635977 100.607649 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 961.979004 101.385916 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1013.547583 102.751511 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1065.998519 103.302222 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 603.733168 146.240099 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 653.628066 147.156874 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 703.453562 147.981552 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 754.398295 148.514773 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 804.948846 149.804741 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 856.542794 150.568747 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 908.201954 151.724756 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 960.133838 152.617845 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1012.452335 153.964008 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1064.578110 155.364827 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 602.391832 196.346522 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 652.234165 197.322457 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 702.178635 198.203561 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 752.800348 199.476494 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 803.742871 200.011116 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 855.181818 201.379203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 906.929845 202.237812 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 958.951652 203.771152 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1011.432907 204.949681 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1063.287671 206.328767 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 601.807096 245.722639 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 650.847842 247.186540 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 700.983157 248.198749 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 751.222801 249.400651 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 802.368288 250.838399 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 853.912192 251.829978 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 905.725225 253.335586 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 957.935957 254.438272 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1010.135156 256.060156 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1062.360485 257.584922 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 600.282069 295.565517 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 650.052062 296.625773 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 699.639273 298.371636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 750.259868 299.569079 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 801.055085 301.189831 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 852.506148 302.597848 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 904.786545 303.769103 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 956.673050 305.581560 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1009.289588 307.042299 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1061.354478 308.680970 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 599.869613 344.411893 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 648.723684 346.445395 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 698.977373 347.929169 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 748.888889 349.864734 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 799.983445 351.273668 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 851.732323 352.974747 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 903.575980 354.798203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 956.156646 356.839732 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1007.875415 358.427741 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1060.383920 360.523618 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 598.545455 393.875077 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 648.323441 395.658451 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 697.673737 398.089899 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 748.243056 399.447650 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 799.241703 401.868687 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 850.523416 403.607897 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 902.858505 405.951564 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 954.901249 407.348468 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1007.203402 410.039941 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1059.151685 411.540262 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 598.282490 443.125171 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 647.082897 445.513089 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 697.187537 447.427984 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 747.639900 449.829549 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 798.230889 451.925117 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 850.044925 454.290349 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 902.246975 456.569307 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 954.056077 458.850918 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1005.688801 460.529237 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1057.716944 463.236043 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 597.068257 492.785951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 646.740648 494.796758 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 696.014354 497.460925 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 746.782009 499.701904 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 797.676649 502.355193 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 848.888179 505.039936 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 900.846221 507.467487 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 952.767794 509.476868 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1004.449529 511.932768 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img7.png 1056.767989 513.663080 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 782.867536 169.829905 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 818.491897 169.535113 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 854.664952 169.623806 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 892.484076 169.244161 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 931.294433 169.122056 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 971.671227 168.894969 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1013.042029 169.194203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1055.847869 168.886898 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1099.728412 169.026462 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1145.835689 168.867491 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 775.105051 202.570909 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 810.597612 202.549325 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 847.598015 202.928394 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 885.750000 203.705214 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 924.999516 203.571221 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 965.645998 204.631342 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1007.401820 204.476054 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1050.470414 205.717949 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1094.879512 206.100976 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1140.859076 206.514092 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 767.005135 235.436457 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 802.731218 236.495259 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 840.180576 237.517986 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 878.488688 238.110407 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 918.083107 239.439024 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 959.328034 239.794913 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1001.322940 241.652561 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1044.964749 242.668740 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1089.825267 243.881835 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1136.322478 245.256553 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 758.636455 269.463914 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 794.995806 270.916589 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 832.236567 272.221912 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 871.048208 273.807166 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 911.203020 275.191275 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 952.528331 277.631700 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 995.293895 278.685575 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1039.214830 281.101178 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1084.627053 282.897101 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1131.438961 284.918831 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 750.547022 303.710554 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 786.582459 305.905922 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 824.476544 308.036357 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 863.617103 310.011941 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 903.965461 312.654605 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 945.990734 314.353528 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 989.002367 317.686982 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1033.621053 319.661842 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1079.253313 322.643731 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1126.687663 325.572098 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 741.817769 338.821901 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 778.679182 341.639033 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 816.429469 344.210628 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 855.936416 347.442004 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 896.611305 349.902098 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 938.767515 353.621135 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 982.712382 356.445925 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1027.277246 360.217054 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1073.893966 363.478617 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1121.521008 366.973307 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 733.442032 374.528156 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 769.976058 378.230868 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 808.483760 381.615066 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 847.925278 385.229465 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 889.156909 389.000000 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 931.895811 392.650913 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 975.664336 396.996066 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1021.358200 401.270037 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1068.103803 405.463087 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1116.666849 410.081510 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 724.307132 411.267569 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 761.740126 415.194159 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 800.158153 419.517145 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 840.108795 424.116567 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 882.041629 428.909579 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 924.442083 433.498333 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 969.306030 438.078498 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1014.853322 443.477998 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1062.441116 448.650783 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1111.199012 453.765926 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 715.837162 448.473196 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 752.785714 453.388095 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 791.984791 458.620118 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 831.847102 463.937115 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 873.692565 469.241855 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 917.302454 475.053129 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 962.032161 480.855779 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1008.687332 486.738544 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1056.092561 492.676038 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1105.695515 499.014949 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 706.450704 486.927920 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 744.318410 492.652720 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 782.853237 498.489209 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 823.996278 504.791961 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 865.568293 511.136585 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 909.784570 517.771897 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 954.849589 524.441246 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1001.489270 531.162661 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1049.652520 537.889920 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img4.png 1099.589183 544.955696 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 121.979212 397.623332 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 154.234215 384.833904 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 186.240070 372.383972 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 218.342648 359.526655 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 250.123647 346.934638 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 282.377762 334.413329 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 313.852881 321.967307 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 345.625841 310.080305 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 377.059165 297.598548 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 408.266254 285.674149 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 129.609380 426.542594 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 162.176794 413.403038 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 194.624288 400.428843 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 226.969150 387.659518 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 259.479292 375.035015 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 291.783984 362.117864 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 324.111341 349.403309 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 355.974290 336.896482 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 387.597172 324.660986 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 418.968231 312.446931 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 137.515957 455.789229 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 170.400061 442.923053 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 203.207675 429.602333 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 236.037206 416.699413 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 268.759211 403.530702 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 301.486884 390.576775 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 334.101487 377.486002 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 366.474394 364.816712 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 398.492320 351.886675 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 430.256529 339.698821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 145.686379 486.115191 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 178.827852 472.785816 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 212.116682 459.195989 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 245.317316 446.011599 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 278.341826 432.707006 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 311.598278 419.381636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 344.366949 406.292453 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 377.374558 392.953180 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 409.553118 380.284450 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 441.792655 367.208875 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 153.727603 517.052562 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 187.542516 503.542516 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 221.365761 489.920362 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 254.803787 476.139759 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 288.256820 462.507526 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 321.718111 449.051692 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 355.029475 435.446409 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 388.082550 422.281087 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 420.890080 408.740483 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 453.220347 395.951642 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 162.616755 548.635198 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 196.489263 534.861895 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 230.412844 521.149157 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 264.452710 507.247078 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 298.434146 493.199187 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 332.034197 479.466078 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 365.867841 465.639567 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 399.434258 451.711701 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 432.362283 438.394541 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 465.085020 424.657220 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 171.439695 581.272366 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 206.008645 567.090769 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 240.208673 553.169385 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 274.322535 539.028169 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 308.808324 524.817087 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 342.948650 510.595396 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 376.891269 496.498353 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 410.751190 482.531372 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 444.327897 468.267811 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 477.136010 455.092832 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 180.761144 614.267872 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 215.562672 600.041629 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 250.162143 585.801322 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 285.098901 571.389242 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 319.509244 556.979412 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 354.059568 542.571109 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 388.472829 527.968004 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 422.576670 513.615240 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 456.315436 499.452796 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 489.626775 485.048682 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 190.351247 648.604041 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 225.453077 633.958061 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 260.679134 619.259843 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 295.534073 604.746038 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 330.610646 590.070722 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 365.415020 575.302654 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 400.002642 560.541612 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 434.606607 545.825325 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 468.603000 531.218000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 502.463039 516.826304 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 200.044714 683.410364 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 235.889354 668.278940 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 271.430902 653.536468 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 306.230831 638.618211 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 341.820780 623.786298 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 377.134652 608.760829 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 412.084400 593.885728 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 446.731092 578.758403 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 481.424685 564.025579 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img12.png 515.855000 549.312000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 57.017923 108.276267 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 109.170330 108.091575 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 158.693587 107.598575 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 207.898518 107.404789 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 255.446753 106.521429 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 303.272841 106.813517 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 349.777778 106.088670 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 396.056911 106.513550 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 440.713823 106.093389 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 485.233611 107.030989 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 59.065217 154.793951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 110.295855 154.298446 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 159.931925 153.791080 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 209.167264 152.202509 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 257.870777 151.672297 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 305.582759 150.854023 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 353.076210 150.566426 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 398.814795 150.134795 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 444.477273 149.827441 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 488.110587 149.342767 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 60.248267 202.230624 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 112.002841 200.930114 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 161.733968 199.643788 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 211.107090 198.233515 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 259.788820 196.843027 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 308.157859 196.322581 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 355.230907 194.634845 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 401.708108 194.693694 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 447.206934 193.224810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 491.638932 193.073783 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 62.059126 249.616967 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 113.425316 248.122785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 163.688694 246.255034 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 213.052288 244.308972 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 262.218710 243.176991 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 310.566053 240.878303 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 358.299417 240.361633 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 404.820152 238.552004 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 450.079123 238.049571 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 495.142259 236.086671 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 63.555556 297.713725 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 115.202322 295.542841 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 165.280124 293.231677 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 215.472879 291.152527 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 264.629827 288.807590 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 313.319036 287.476498 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 361.149111 285.084815 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 407.807479 283.831717 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 453.938076 281.561265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 498.342466 280.391096 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 65.271039 345.940221 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 117.043125 343.607500 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 167.611352 340.939394 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 217.630025 338.346106 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 267.267672 335.870877 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 315.837635 333.105745 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 364.239746 331.052734 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 411.415154 328.447514 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 456.839975 326.671727 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 502.098710 323.989677 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 66.963528 395.124005 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 119.066173 391.892840 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 169.750885 388.957497 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 220.089882 385.951529 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 269.708812 382.664112 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 319.022199 380.011358 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 367.029445 376.698754 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 414.187100 374.166420 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 460.570122 371.215447 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 505.192217 368.627948 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 68.725576 444.346609 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 121.077020 440.823864 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 172.000983 437.332187 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 222.446483 434.308869 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 272.297284 430.370221 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 321.423131 426.894217 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 369.227000 423.490000 2 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 417.317771 419.884036 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 463.553655 416.605495 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 508.995192 413.396635 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 70.750996 494.146414 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 123.113329 489.906632 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 174.225619 486.198208 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 224.839738 482.322399 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 274.711443 478.573134 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 324.216676 474.200212 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 372.626673 470.504758 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 420.208080 466.314887 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 466.866412 462.185115 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 511.891787 458.588790 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 72.445783 543.855422 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 125.508403 539.436134 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 176.948133 535.271093 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 227.344928 530.886957 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 277.347804 526.661282 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 326.943015 522.340074 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 375.118943 517.720509 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 423.360795 512.988636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 469.516452 508.773486 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img0.png 515.495256 504.111954 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 705.620043 247.722937 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 739.210600 248.322176 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 774.388115 249.168059 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 810.185681 249.663476 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 847.396817 250.811745 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 885.823087 251.136763 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 925.543974 252.803474 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 965.825133 253.035239 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1007.568932 254.553398 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1050.322643 255.396408 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 698.230305 280.517960 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 732.558824 281.705510 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 767.551150 282.966011 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 804.002510 284.141423 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 841.245686 285.352095 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 879.856936 287.159441 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 919.761838 288.189873 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 960.775623 290.208449 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1003.223062 291.425961 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1046.146510 293.319281 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 691.441190 313.980009 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 725.119297 315.791484 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 761.030758 317.681802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 797.208098 319.303439 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 835.116383 321.349148 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 873.937984 323.039216 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 914.305729 325.448958 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 955.741473 327.302865 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 998.538622 329.832289 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1042.434821 331.861607 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 683.579122 347.654515 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 718.528543 350.231299 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 753.726843 352.489160 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 790.885062 354.925308 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 828.660313 357.383675 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 868.148275 360.269245 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 908.921569 362.551821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 950.583645 365.784112 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 994.051478 368.587226 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1037.787273 371.737576 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 676.642897 382.325594 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 711.144028 385.053084 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 747.205342 388.143600 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 783.905914 391.374104 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 822.399788 394.544008 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 862.101170 397.772195 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 902.899960 401.332543 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 945.513228 404.706349 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 988.755022 408.494978 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1033.716609 412.099884 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 668.686802 417.437056 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 703.870032 420.953455 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 739.725054 424.657809 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 777.338250 428.522779 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 815.754123 432.485007 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 855.784946 436.457587 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 897.050469 440.606808 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 939.821023 445.111742 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 984.008608 449.285509 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1028.850824 453.912471 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 661.042688 453.439983 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 696.107467 457.721563 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 732.756642 461.987537 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 770.118927 466.691806 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 809.114929 471.183658 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 849.560241 475.971386 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 891.055405 481.015752 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 934.347261 486.547039 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 978.430260 491.383452 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1024.112420 496.517131 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 652.845625 490.514375 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 688.509246 495.278450 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 724.713268 500.579697 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 763.133795 505.681371 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 801.951220 511.195122 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 842.912652 516.618317 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 885.031524 522.183888 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 928.234927 528.189882 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 972.971893 533.991124 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1018.765625 540.061655 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 644.942411 528.385268 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 680.302817 533.845070 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 717.444777 539.659675 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 755.544928 545.440580 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 795.084523 551.706915 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 836.451705 558.369886 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 878.449286 564.614576 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 922.164464 571.287812 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 967.234547 577.945385 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1013.380172 584.604310 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 636.103741 567.278061 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 672.448970 573.380931 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 709.133728 580.049704 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 748.202561 586.549611 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 788.161084 593.755748 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 829.400773 600.654273 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 872.000000 607.720524 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 915.974121 615.131015 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 961.087224 622.386978 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img5.png 1007.670813 629.776100 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 314.486192 98.216570 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 366.072091 98.666323 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 418.123077 98.938462 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 470.366292 99.733708 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 522.096725 99.722772 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 575.288970 100.488605 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 627.017781 101.006401 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 679.894706 101.466984 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 732.232939 102.266606 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 785.083035 102.624195 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 312.686131 150.357664 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 364.409353 150.865473 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 416.518467 151.681533 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 468.606157 151.957537 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 520.889734 152.984791 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 573.074975 153.227905 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 625.393675 153.953108 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 677.724623 154.572618 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 730.284759 155.002389 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 782.881356 155.760401 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 311.185532 202.344943 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 363.233492 203.233492 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 415.315104 203.781250 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 467.371859 204.877243 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 519.609663 205.174810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 571.625229 206.160878 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 623.822760 206.754964 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 676.004479 207.250280 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 728.500000 207.959393 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 781.064906 208.473047 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 310.329630 254.632099 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 362.106620 255.252265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 414.134405 256.337621 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 466.458722 256.629161 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 518.277666 257.888665 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 570.656889 258.323329 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 622.190269 259.397967 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 674.834696 259.798481 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 726.742616 260.649789 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 779.682238 261.068051 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 309.083612 306.546265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 361.344330 307.208247 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 413.495829 307.800741 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 465.328814 309.204068 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 517.577677 309.467572 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 569.209145 310.491754 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 621.488746 311.122186 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 673.085630 312.244094 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 725.712546 312.754568 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 778.035461 313.805471 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 308.200650 358.437043 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 359.915550 359.692646 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 412.544218 359.815571 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 464.650118 360.652482 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 516.315484 361.452258 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 568.500752 362.141353 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 619.775742 363.335079 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 672.469197 363.860610 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 724.036364 365.140260 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 776.848564 365.482376 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 306.909361 410.476226 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 358.772139 410.849751 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 411.112385 411.756881 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 463.381761 412.305660 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 515.414903 413.217111 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 566.971347 413.848711 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 619.106396 414.699877 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 670.752232 415.971726 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 722.979565 416.679631 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 775.454194 417.793548 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 305.886707 462.168429 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 357.868908 463.016807 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 409.938882 463.542263 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 461.927856 464.256513 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 514.043390 464.781017 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 565.887518 465.785196 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 617.705446 466.717203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 669.721866 467.628571 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 721.520219 468.824044 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 774.037820 469.844271 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 304.919127 514.137356 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 356.644820 514.692389 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 408.480306 515.517505 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 460.841808 516.204802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 512.850208 516.919556 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 564.602596 517.673053 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 616.632555 518.684066 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 668.414955 519.732950 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 720.531500 521.000887 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 773.077904 522.406920 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 304.297245 565.508648 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 355.640523 566.789542 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 407.573308 567.480263 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 459.554988 568.419874 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 511.643501 569.311454 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 563.765672 569.874627 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 615.506135 570.830521 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 667.554566 571.897550 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 719.653169 573.140845 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img6.png 772.378540 574.432662 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 41.054671 241.910035 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 89.836860 240.488737 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 137.196226 238.977358 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 183.766830 237.588739 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 230.269520 236.111111 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 275.676385 235.248785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 320.955771 233.158950 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 365.418006 232.922026 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 409.281481 231.123810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 452.127018 230.574035 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 43.412748 287.188088 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 91.762653 285.762653 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 139.129336 283.641724 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 185.577143 281.928254 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 231.916667 280.222222 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 278.008362 278.287108 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 323.107299 277.202745 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 368.039133 275.124624 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 411.291667 274.256366 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 454.731258 272.249047 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 45.198657 333.035814 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 93.640900 330.831703 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 140.931275 328.664531 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 187.706834 326.528951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 234.098039 324.423203 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 279.958763 322.360825 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 325.684335 320.034019 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 369.873442 320.038351 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 414.104704 316.112291 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 456.583041 314.822222 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 47.291925 378.508075 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 95.893747 376.141840 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 143.179560 373.594870 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 189.764977 371.048716 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 236.096535 368.691584 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 282.146552 365.962644 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 327.766405 363.702742 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 372.440845 361.114085 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 416.354854 358.932139 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 459.289005 356.515183 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 49.538999 424.490105 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 97.927840 421.659218 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 145.238181 418.715141 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 191.971728 415.956680 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 238.368760 413.034622 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 284.381164 410.076430 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 329.893730 407.343252 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 374.481599 404.390731 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 418.370262 401.183673 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 461.153929 399.053821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 51.633628 470.195280 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 100.121766 466.979037 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 147.612570 463.870544 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 194.315882 460.658235 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 240.573017 457.575919 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 286.424479 454.302951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 332.015748 450.878861 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 376.410924 447.801401 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 420.483855 444.368819 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 463.542705 440.912811 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 53.784326 516.112226 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 102.507737 512.342535 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 149.931752 508.741045 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 196.735753 505.411397 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 242.827117 502.037300 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 288.752887 498.400693 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 333.986811 495.005995 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 378.656287 490.884841 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 422.846847 487.219219 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 465.697574 483.696496 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 55.962675 562.056506 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 104.969444 557.753241 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 152.511638 553.954342 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 199.053309 550.172181 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 245.209046 546.462918 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 290.900156 542.609204 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 336.176649 538.532970 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 380.961017 534.571186 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 424.822446 530.649079 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 467.260500 526.346746 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 58.018914 608.175107 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 107.596023 603.121591 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 154.991018 598.780240 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 201.816667 594.844608 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 247.899312 590.853143 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 293.332249 586.720260 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 338.493681 582.515798 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 382.775449 578.061078 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 426.722704 573.525130 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 469.862947 569.269772 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 60.122248 654.243736 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 109.562130 648.899408 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 157.980249 643.847110 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 204.290011 639.335982 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 250.668072 635.092169 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 295.955821 630.871622 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 340.777215 626.288608 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 385.036058 621.670673 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 428.886332 616.951620 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img1.png 472.062500 612.084375 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 133.706282 168.856819 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 170.184165 167.489960 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 207.017581 165.463979 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 244.167728 164.148620 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 281.010242 162.474860 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 318.416040 161.093358 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 355.165275 159.545353 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 392.870189 158.343118 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 429.452917 156.936029 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 466.855398 156.219152 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 135.152946 205.877193 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 171.510140 203.831513 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 208.890390 202.351688 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 245.454869 201.039984 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 282.694342 199.663141 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 319.885163 197.883785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 357.509848 196.759707 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 394.495886 195.308792 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 431.666831 194.420662 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 468.501857 192.788279 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 136.439798 242.648363 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 173.141715 241.088405 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 210.192908 239.517730 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 247.137202 237.903284 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 284.334732 236.324009 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 321.564878 235.047805 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 358.842318 233.509434 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 396.254801 232.444965 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 433.548990 231.120419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 470.152187 230.112043 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 137.913535 279.605587 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 174.488759 277.961331 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 211.520925 276.358709 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 248.623771 274.946131 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 286.012195 273.244841 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 323.296400 272.029200 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 360.710537 270.748667 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 398.044541 269.057154 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 434.881771 268.331470 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 472.176000 266.296000 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 139.412893 316.674827 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 176.048926 314.994016 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 213.095325 313.622365 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 250.109457 311.911871 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 287.603843 310.460659 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 324.838323 309.047049 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 362.490097 307.453247 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 399.398363 306.304910 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 436.948902 304.403056 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 473.421693 303.558730 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 140.750204 353.596893 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 177.728705 352.130970 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 214.631292 350.292785 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 251.795091 349.025114 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 289.107257 347.350478 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 326.430244 345.752195 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 363.891667 344.216270 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 401.275737 342.562358 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 438.111686 341.542906 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 475.841975 339.679835 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 142.512938 390.539390 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 179.259850 388.891182 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 216.246231 387.563819 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 253.131245 385.979778 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 290.861300 384.198142 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 327.991193 382.695288 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 365.620509 380.996734 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 402.868987 379.552210 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 440.276580 377.770115 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 476.693449 376.583419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 144.087531 427.275306 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 180.663934 426.045667 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 217.729412 424.384615 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 254.836867 423.037102 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 292.106810 421.210821 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 329.793362 419.717345 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 367.082141 417.948508 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 404.424163 416.158240 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 441.460407 414.547382 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 478.981439 412.668213 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 145.631843 464.253656 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 182.301119 463.047304 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 219.321147 461.706810 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 256.354593 459.919086 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 293.679498 458.390795 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 331.041529 456.607362 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 368.710751 454.888225 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 405.896595 453.011349 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 442.817073 450.716802 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 479.757377 449.819126 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 147.718792 501.273212 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 184.027577 499.713723 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 220.900243 498.356042 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 257.873272 497.170507 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 295.134795 495.341860 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 332.818798 493.811047 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 369.922383 491.764892 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 407.220859 489.590491 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 444.497799 488.165933 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img11.png 481.216783 485.552448 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 411.800748 301.396632 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 453.583031 302.657895 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 495.051988 303.072630 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 535.048017 304.115344 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 575.454313 304.746326 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 613.789947 305.827457 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.443427 306.021583 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 690.851639 307.014754 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 729.463497 307.432371 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 766.408882 308.185299 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 411.595433 343.239121 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 453.825874 343.396503 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 494.576538 343.998569 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 535.632113 344.129014 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.903160 344.753313 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.779108 344.976061 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 652.812076 345.830393 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 691.897577 345.943833 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 729.185382 346.501661 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 766.930957 346.766697 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 411.909707 384.316027 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 453.132634 384.531966 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 495.042381 384.570784 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.969200 384.769498 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 575.348749 384.842764 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 613.983574 385.239159 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.641677 385.052538 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 691.154219 385.585062 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 730.003780 385.453564 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 767.018018 385.844273 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 411.010062 426.056540 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 453.153409 425.659091 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 494.384977 425.570636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 535.269091 425.203636 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.915834 425.176749 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.727144 424.801559 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.148022 424.989768 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.185127 424.858650 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 729.808388 425.022204 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 767.392857 424.561077 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 410.370000 468.036000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 452.776954 467.124135 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 494.386167 466.576369 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.783558 466.252206 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 575.425049 465.574951 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.300525 465.456037 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.873810 464.750000 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 691.720879 464.742857 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 730.106539 464.010974 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 767.471826 463.918858 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 410.162266 509.790206 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 452.297000 509.103000 1 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 494.032764 507.914530 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.922249 507.287081 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.944229 506.323957 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.760278 505.480076 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.406932 505.156611 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.375439 504.332018 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 730.204647 504.103664 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 767.988121 503.086393 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 409.801835 552.064220 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 452.053398 550.733549 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 493.764505 549.738908 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.558301 548.359341 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.851852 547.304233 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.580116 546.459545 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 654.113234 545.433364 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.463529 544.602941 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 730.669715 543.486283 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 768.054908 543.043261 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 409.478092 594.433167 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 451.582596 592.980826 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 493.526116 591.370370 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.566132 590.002004 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.891959 588.595052 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.929657 587.306134 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 653.960275 586.077979 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.871582 584.929992 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 731.075446 583.698217 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 768.703422 582.361217 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 408.962818 636.808219 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 451.499476 635.055031 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 493.020808 633.127295 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 534.185048 631.754038 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.857252 630.154259 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.761752 628.394231 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 654.341050 626.902880 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 692.788504 625.551953 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 731.352644 623.833103 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 768.964461 622.504902 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 408.995178 679.052555 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 451.286104 677.290645 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 492.915254 675.373380 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 533.995570 673.186047 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 574.802809 671.578652 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 614.810612 669.645714 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 654.205402 667.791457 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 693.244163 666.023349 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 731.477035 664.050104 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img3.png 769.406926 662.199134 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 186.220582 281.585758 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 219.935150 257.582707 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 253.362547 234.154307 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 286.876701 211.038265 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 319.688632 187.981503 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 352.825800 165.146893 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 384.651256 142.795413 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 417.127890 120.274566 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 448.347710 98.286192 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 480.260181 75.996606 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 211.813258 312.072757 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 244.895688 288.745380 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 278.215420 265.382086 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 310.695108 242.437239 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 343.643882 219.373840 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 375.395233 197.239656 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 407.326188 175.020278 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 438.525492 152.821263 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 469.922828 130.802482 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 500.345295 109.262907 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 236.690551 342.480315 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 269.579035 318.952579 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 301.964582 295.793179 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 334.486111 273.103395 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 366.139966 250.466552 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 397.954420 228.388582 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 428.803443 206.048513 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 459.926161 184.484716 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 490.175280 162.632820 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 520.593670 140.874251 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 260.952014 371.131105 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 293.146361 348.369522 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 325.284036 325.344160 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 356.737814 302.803545 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 388.237856 280.750419 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 419.185863 258.338277 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 450.063086 236.651748 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 480.050791 214.821815 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 510.325690 193.570701 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 539.922832 171.944425 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 284.414474 399.833333 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 316.439732 376.622024 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 347.657529 354.017649 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 378.925014 331.712367 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 409.733793 309.506107 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 440.065753 287.905023 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 470.291572 265.828018 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 500.146760 244.923888 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 529.612558 223.148965 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 558.599749 202.376412 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 307.180233 427.287468 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 338.515786 404.342878 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 369.590029 381.874637 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 400.055316 359.625704 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 430.528662 337.732059 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 460.262030 315.826632 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 489.934764 295.127039 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 519.170066 272.964051 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 548.247477 252.061430 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 576.702519 231.181076 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 329.371294 453.698824 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 360.036415 431.341270 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 390.646211 408.799128 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 420.773720 386.882154 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 450.480769 364.714497 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 479.875985 343.931149 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 508.909873 321.836601 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 537.948540 301.055839 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 566.113097 279.960555 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 594.441289 259.702226 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 350.837404 479.840617 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 381.096464 457.000000 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 411.059259 435.180392 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 440.655052 412.971690 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 469.980579 391.689630 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 498.949139 369.953144 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 527.561286 349.113387 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 555.651342 327.919456 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 583.816299 307.429192 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 611.377934 286.735748 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 371.970353 504.484036 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 401.488326 482.743666 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 431.002443 460.181729 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 460.004195 438.922819 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 488.910013 417.018527 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 517.164992 396.238693 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 545.577342 374.815904 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 573.137751 354.455422 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 600.740126 333.535253 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 627.956121 313.945151 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 392.132898 529.083878 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 421.482955 506.781250 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 450.174537 485.351464 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 478.786033 463.397474 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 507.102445 442.511641 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 535.377049 421.083789 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 562.723848 400.581567 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 590.160636 379.296223 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 617.177298 359.641386 0 +/home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/img8.png 643.892129 338.873352 0 diff --git a/wpical/src/main/native/resources/testcalibration.avi b/wpical/src/main/native/resources/testcalibration.avi new file mode 100644 index 00000000000..40fe6d8b75a Binary files /dev/null and b/wpical/src/main/native/resources/testcalibration.avi differ diff --git a/wpical/src/main/native/resources/testcalibration.mp4 b/wpical/src/main/native/resources/testcalibration.mp4 new file mode 100644 index 00000000000..6e4a8573a6f Binary files /dev/null and b/wpical/src/main/native/resources/testcalibration.mp4 differ diff --git a/wpical/src/main/native/resources/wpical-128.png b/wpical/src/main/native/resources/wpical-128.png new file mode 100644 index 00000000000..136b764d967 Binary files /dev/null and b/wpical/src/main/native/resources/wpical-128.png differ diff --git a/wpical/src/main/native/resources/wpical-16.png b/wpical/src/main/native/resources/wpical-16.png new file mode 100644 index 00000000000..dcd136a3c5f Binary files /dev/null and b/wpical/src/main/native/resources/wpical-16.png differ diff --git a/wpical/src/main/native/resources/wpical-256.png b/wpical/src/main/native/resources/wpical-256.png new file mode 100644 index 00000000000..4c8cb2f89ef Binary files /dev/null and b/wpical/src/main/native/resources/wpical-256.png differ diff --git a/wpical/src/main/native/resources/wpical-32.png b/wpical/src/main/native/resources/wpical-32.png new file mode 100644 index 00000000000..07a5799cea4 Binary files /dev/null and b/wpical/src/main/native/resources/wpical-32.png differ diff --git a/wpical/src/main/native/resources/wpical-48.png b/wpical/src/main/native/resources/wpical-48.png new file mode 100644 index 00000000000..82711988629 Binary files /dev/null and b/wpical/src/main/native/resources/wpical-48.png differ diff --git a/wpical/src/main/native/resources/wpical-512.png b/wpical/src/main/native/resources/wpical-512.png new file mode 100644 index 00000000000..aa31a11303b Binary files /dev/null and b/wpical/src/main/native/resources/wpical-512.png differ diff --git a/wpical/src/main/native/resources/wpical-64.png b/wpical/src/main/native/resources/wpical-64.png new file mode 100644 index 00000000000..5816f8f50ab Binary files /dev/null and b/wpical/src/main/native/resources/wpical-64.png differ diff --git a/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h b/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h new file mode 100644 index 00000000000..924e0889af4 --- /dev/null +++ b/wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h @@ -0,0 +1,301 @@ +// -*- mode: C; c-basic-offset: 2 -*- +// Copyright 2011 Oblong Industries +// 2017 Dima Kogan +// License: GNU LGPL . + +#pragma once + +#include +#include + +typedef void (dogleg_callback_t)(const double* p, + double* x, + cholmod_sparse* Jt, + void* cookie); +typedef void (dogleg_callback_dense_t)(const double* p, + double* x, + double* J, + void* cookie); + +// an operating point of the solver +typedef struct +{ + // The pointers in this structure are all indices into a single "pool" array + // (see allocOperatingPoint()). I thus don't need to store the pointers at + // all, and can just index that one array directly, but that would break my + // ABI + double* p; + double* x; + double norm2_x; + union + { + cholmod_sparse* Jt; + double* J_dense; // row-first: grad0, grad1, grad2, ... + }; + double* Jt_x; + + // the cached update vectors. It's useful to cache these so that when a step is rejected, we can + // reuse these when we retry + double* updateCauchy; + union + { + cholmod_dense* updateGN_cholmoddense; + double* updateGN_dense; + }; + double updateCauchy_lensq, updateGN_lensq; // update vector lengths + + // whether the current update vectors are correct or not + int updateCauchy_valid, updateGN_valid; + + int didStepToEdgeOfTrustRegion; + + double* step_to_here; + double step_to_here_len_sq; + +} dogleg_operatingPoint_t; + +// The newer APIs ( dogleg_...2() ) take a dogleg_parameters2_t argument for +// their settings, while the older ones use a global set of parameters specified +// with dogleg_set_...(). This global-parameters approach doesn't work if we +// have multiple users of libdogleg in the same application +typedef struct +{ + int max_iterations; + int dogleg_debug; + + // it is cheap to reject a too-large trust region, so I start with something + // "large". The solver will quickly move down to something reasonable. Only the + // user really knows if this is "large" or not, so they should change this via + // the API + double trustregion0; + + // These are probably OK to leave alone. Tweaking them can maybe result in + // slightly faster convergence + double trustregion_decrease_factor; + double trustregion_decrease_threshold; + double trustregion_increase_factor; + double trustregion_increase_threshold; + + // The termination thresholds. Documented in the header + double Jt_x_threshold; + double update_threshold; + double trustregion_threshold; +} dogleg_parameters2_t; + +// solver context. This has all the internal state of the solver +typedef struct +{ + cholmod_common common; + + union + { + dogleg_callback_t* f; + dogleg_callback_dense_t* f_dense; + }; + void* cookie; + + // between steps, beforeStep contains the operating point of the last step. + // afterStep is ONLY used while making the step. Externally, use beforeStep + // unless you really know what you're doing + dogleg_operatingPoint_t* beforeStep; + dogleg_operatingPoint_t* afterStep; + + // The result of the last JtJ factorization performed. Note that JtJ is not + // necessarily factorized at every step, so this is NOT guaranteed to contain + // the factorization of the most recent JtJ + union + { + cholmod_factor* factorization; + + // This is a factorization of JtJ, stored as a packed symmetric matrix + // returned by dpptrf('L', ...) + double* factorization_dense; + }; + + // Have I ever seen a singular JtJ? If so, I add this constant to the diagonal + // from that point on. This is a simple and fast way to deal with + // singularities. This constant starts at 0, and is increased every time a + // singular JtJ is encountered. This is suboptimal but works for me for now. + double lambda; + + // Are we using sparse math (cholmod)? + int is_sparse; + int Nstate, Nmeasurements; + + const dogleg_parameters2_t* parameters; + +} dogleg_solverContext_t; + +#ifdef __cplusplus +extern "C" { +#endif +// Fills in the given structure with the default parameter set +void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters); + +void dogleg_setMaxIterations(int n); +void dogleg_setTrustregionUpdateParameters(double downFactor, double downThreshold, + double upFactor, double upThreshold); + +// The argument is a bit-mapped integer. Should be a bit-field structure or +// enum, but for API backwards-compatibility, I keep this an integer. +// +// if(debug == 0 ): no diagnostic output +// if(debug & DOGLEG_DEBUG_VNLOG): output vnlog diagnostics to stdout +// if(debug & ~DOGLEG_DEBUG_VNLOG): output human-oriented diagnostics to stderr +#define DOGLEG_DEBUG_VNLOG (1 << 30) +void dogleg_setDebug(int debug); + + +// The following parameters reflect the scaling of the problem being solved, so +// the user is strongly encouraged to tweak these. The defaults are set +// semi-arbitrarily + +// The size of the trust region at start. It is cheap to reject a too-large +// trust region, so this should be something "large". Say 10x the length of an +// "expected" step size +void dogleg_setInitialTrustregion(double t); + +// termination thresholds. These really depend on the scaling of the input +// problem, so the user should set these appropriately +// +// Jt_x threshold: +// The function being minimized is E = norm2(x) where x is a function of the state p. +// dE/dp = 2*Jt*x where Jt is transpose(dx/dp). +// if( for every i fabs(Jt_x[i]) < JT_X_THRESHOLD ) +// { we are done; } +// +// update threshold: +// if(for every i fabs(state_update[i]) < UPDATE_THRESHOLD) +// { we are done; } +// +// trust region threshold: +// if(trustregion < TRUSTREGION_THRESHOLD) +// { we are done; } +// +// to leave a particular threshold alone, use a value <= 0 here +void dogleg_setThresholds(double Jt_x, double update, double trustregion); + + +// The main optimization function. Initial estimate of the solution passed in p, +// final optimized solution returned in p. p has Nstate variables. There are +// Nmeas measurements, the jacobian matrix has NJnnz non-zero entries. +// +// The evaluation function is given in the callback f; this function is passed +// the given cookie +// +// If we want to get the full solver state when we're done, a non-NULL +// returnContext pointer can be given. If this is done then THE USER IS +// RESPONSIBLE FOR FREEING ITS MEMORY WITH dogleg_freeContext() +double dogleg_optimize(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, void* cookie, + dogleg_solverContext_t** returnContext); +double dogleg_optimize2(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, void* cookie, + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext); + +// Main optimization function if we're using dense linear algebra. The arguments +// are very similar to the sparse version: dogleg_optimize() +double dogleg_optimize_dense(double* p, unsigned int Nstate, + unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie, + dogleg_solverContext_t** returnContext); +double dogleg_optimize_dense2(double* p, unsigned int Nstate, + unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie, + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext); + +// Compute the cholesky decomposition of JtJ. This function is only exposed if +// you need to touch libdogleg internals via returnContext. Sometimes after +// computing an optimization you want to do stuff with the factorization of JtJ, +// and this call ensures that the factorization is available. Most people don't +// need this function. If the comment wasn't clear, you don't need this +// function. +void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx); + +// Generates a plain text table that contains gradient checks. This table can be +// easily parsed with "vnlog" tools +void dogleg_testGradient(unsigned int var, const double* p0, + unsigned int Nstate, unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, void* cookie); +void dogleg_testGradient_dense(unsigned int var, const double* p0, + unsigned int Nstate, unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie); + + +// If we want to get the full solver state when we're done optimizing, we can +// pass a non-NULL returnContext pointer to dogleg_optimize(). If we do this, +// then the user MUST call dogleg_freeContext() to deallocate the pointer when +// the USER is done +void dogleg_freeContext(dogleg_solverContext_t** ctx); + + +// Computes outlierness factors. This function is experimental, and subject to +// change. +bool dogleg_getOutliernessFactors( // output + double* factors, // Nfeatures factors + + // output, input + double* scale, // if <0 then I recompute + + // inputs + // if outliers are grouped into features, the feature size is + // stated here + int featureSize, + int Nfeatures, + int NoutlierFeatures, // how many outliers we already have + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx ); + + + + +// This stuff is experimental, and subject to change. +struct dogleg_outliers_t +{ + unsigned char marked : 1; +}; +bool dogleg_markOutliers(// output, input + struct dogleg_outliers_t* markedOutliers, + double* scale, // if <0 then I recompute + + // output, input + int* Noutliers, // number of outliers before and after this call + + // input + double (getConfidence)(int i_feature_exclude), + + // if outliers are grouped into features, the feature size is + // stated here + int featureSize, + int Nfeatures, + + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx); + +void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude), + double* scale, // if <0 then I recompute + + // if outliers are grouped into features, the feature size + // is stated here + int featureSize, + int Nfeatures, + int Noutliers, // how many outliers we already have + + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx); + +double dogleg_getOutliernessTrace_newFeature_sparse(const double* JqueryFeature, + int istateActive, + int NstateActive, + int featureSize, + int NoutlierFeatures, + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx); + +#ifdef __cplusplus +} // extern "C" +#endif diff --git a/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.cpp b/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.cpp new file mode 100644 index 00000000000..4012c48cb4d --- /dev/null +++ b/wpical/src/main/native/thirdparty/libdogleg/src/dogleg.cpp @@ -0,0 +1,2541 @@ +// -*- mode: C; c-basic-offset: 2 -*- +// Copyright 2011 Oblong Industries +// 2017-2018 Dima Kogan +// License: GNU LGPL . + +// Apparently I need this in MSVC to get constants +#define _USE_MATH_DEFINES + +#include +#include +#include +#include +#include +#include +#include +#include "dogleg.h" + +#if (CHOLMOD_VERSION > (CHOLMOD_VER_CODE(2,2))) && (CHOLMOD_VERSION < (CHOLMOD_VER_CODE(4,0))) +#include +#endif + + +// Any non-vnlog bit mask +#define DOGLEG_DEBUG_OTHER_THAN_VNLOG (~DOGLEG_DEBUG_VNLOG) +#define SAY_NONEWLINE(fmt, ...) fprintf(stderr, "libdogleg at %s:%d: " fmt, __FILE__, __LINE__, ## __VA_ARGS__) +#define SAY(fmt, ...) do { SAY_NONEWLINE(fmt, ## __VA_ARGS__); fprintf(stderr, "\n"); } while(0) + +// This REQUIRES that a "dogleg_solverContext_t* ctx" be available +#define SAY_IF_VERBOSE(fmt,...) do { if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_OTHER_THAN_VNLOG ) SAY(fmt, ##__VA_ARGS__); } while(0) + +// I do this myself because I want this to be active in all build modes, not just !NDEBUG +#define ASSERT(x) do { if(!(x)) { SAY("ASSERTION FAILED: " #x " is not true"); exit(1); } } while(0) + +// used to consolidate the casts +#define P(A, index) ((unsigned int*)((A)->p))[index] +#define I(A, index) ((unsigned int*)((A)->i))[index] +#define X(A, index) ((double* )((A)->x))[index] + + +////////////////////////////////////////////////////////////////////////////////////////// +// vnlog debugging stuff +// +// This is used if the user calls dogleg_setDebug(DOGLEG_DEBUG_VNLOG | stuff) +////////////////////////////////////////////////////////////////////////////////////////// +#define VNLOG_DEBUG_STEP_TYPE_LIST(_) \ + _(STEPTYPE_CAUCHY, "cauchy") \ + _(STEPTYPE_GAUSSNEWTON, "gaussnewton") \ + _(STEPTYPE_INTERPOLATED, "interpolated") \ + _(STEPTYPE_FAILED, "failed") +#define VNLOG_DEBUG_STEP_TYPE_NAME_COMMA(name,shortname) name, +typedef enum { VNLOG_DEBUG_STEP_TYPE_LIST(VNLOG_DEBUG_STEP_TYPE_NAME_COMMA) + STEPTYPE_UNINITIALIZED } vnlog_debug_step_type_t; +#define VNLOG_DEBUG_FIELDS(_) \ + _(double, norm2x_before, INFINITY) \ + _(double, norm2x_after, INFINITY) \ + _(double, step_len_cauchy, INFINITY) \ + _(double, step_len_gauss_newton, INFINITY) \ + _(double, step_len_interpolated, INFINITY) \ + _(double, k_cauchy_to_gn, INFINITY) \ + _(double, step_len, INFINITY) \ + _(vnlog_debug_step_type_t, step_type, STEPTYPE_UNINITIALIZED) \ + _(double, step_direction_change_deg, INFINITY) \ + _(double, expected_improvement, INFINITY) \ + _(double, observed_improvement, INFINITY) \ + _(double, rho, INFINITY) \ + _(double, trustregion_before, INFINITY) \ + _(double, trustregion_after, INFINITY) +static struct vnlog_debug_data_t +{ +#define VNLOG_DEBUG_DECLARE_FIELD(type, name, initialvalue) type name; + VNLOG_DEBUG_FIELDS(VNLOG_DEBUG_DECLARE_FIELD) +} vnlog_debug_data; +static void vnlog_debug_reset(void) +{ +#define VNLOG_DEBUG_RESET_FIELD(type, name, initialvalue) vnlog_debug_data.name = initialvalue; + VNLOG_DEBUG_FIELDS(VNLOG_DEBUG_RESET_FIELD); +} +static void vnlog_debug_emit_legend(void) +{ +#define VNLOG_DEBUG_SPACE_FIELD_NAME(type, name, initialvalue) " " #name + + vnlog_debug_reset(); + printf("# iteration step_accepted" VNLOG_DEBUG_FIELDS(VNLOG_DEBUG_SPACE_FIELD_NAME) "\n"); +} + +static void vnlog_emit_double(double x) +{ + if(x == INFINITY) printf("- "); + else printf("%g ", x); +} + +static void vnlog_emit_int(int x) +{ + if(x == -1) printf("- "); + else printf("%d ", x); +} + +static void vnlog_emit_vnlog_debug_step_type_t(vnlog_debug_step_type_t x) +{ +#define VNLOG_DEBUG_STEP_TYPE_SWITCH_EMIT(name,shortname) case name: printf(shortname " "); break; + switch(x) + { + VNLOG_DEBUG_STEP_TYPE_LIST(VNLOG_DEBUG_STEP_TYPE_SWITCH_EMIT) + default: printf("- "); + } +} +static void vnlog_debug_emit_record(int iteration, int step_accepted) +{ +#define VNLOG_DEBUG_EMIT_FIELD(type, name, initialvalue) vnlog_emit_ ## type(vnlog_debug_data.name); + + printf("%d %d ", iteration, step_accepted); + VNLOG_DEBUG_FIELDS(VNLOG_DEBUG_EMIT_FIELD); + printf("\n"); + fflush(stdout); + vnlog_debug_reset(); +} + +// Default parameters. Used only by the original API, which uses a global set of +// parameters +#define PARAMETERS_DEFAULT \ + { \ + .max_iterations = 100, \ + .dogleg_debug = 0, \ + .trustregion0 = 1.0e3, \ + .trustregion_decrease_factor = 0.1, \ + .trustregion_decrease_threshold = 0.25, \ + .trustregion_increase_factor = 2, \ + .trustregion_increase_threshold = 0.75, \ + .Jt_x_threshold = 1e-8, \ + .update_threshold = 1e-8, \ + .trustregion_threshold = 1e-8 \ + } + +static const dogleg_parameters2_t parameters_default = PARAMETERS_DEFAULT; +static dogleg_parameters2_t parameters_global = PARAMETERS_DEFAULT; +void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters) +{ + *parameters = parameters_default; +} + +// if I ever see a singular JtJ, I factor JtJ + LAMBDA*I from that point on +#define LAMBDA_INITIAL 1e-10 + +// these parameters likely should be messed with +void dogleg_setDebug(int debug) +{ + parameters_global.dogleg_debug = debug; +} +void dogleg_setInitialTrustregion(double t) +{ + parameters_global.trustregion0 = t; +} +void dogleg_setThresholds(double Jt_x, double update, double trustregion) +{ + if(Jt_x > 0.0) parameters_global.Jt_x_threshold = Jt_x; + if(update > 0.0) parameters_global.update_threshold = update; + if(trustregion > 0.0) parameters_global.trustregion_threshold = trustregion; +} + +// these parameters likely should not be messed with. +void dogleg_setMaxIterations(int n) +{ + parameters_global.max_iterations = n; +} +void dogleg_setTrustregionUpdateParameters(double downFactor, double downThreshold, + double upFactor, double upThreshold) +{ + parameters_global.trustregion_decrease_factor = downFactor; + parameters_global.trustregion_decrease_threshold = downThreshold; + parameters_global.trustregion_increase_factor = upFactor; + parameters_global.trustregion_increase_threshold = upThreshold; +} + + + + +////////////////////////////////////////////////////////////////////////////////////////// +// general boring linear algebra stuff +// should really come from BLAS or libminimath +////////////////////////////////////////////////////////////////////////////////////////// +static double norm2(const double* x, unsigned int n) +{ + double result = 0; + for(unsigned int i=0; inrow); + for(unsigned int i=0; incol; i++) + { + for(unsigned int j=P(A, i); jncol; i++) + { + double dotproduct = 0.0; + for(unsigned int j=P(At, i); j 0; + + double* x0 = static_cast(malloc(Nmeas * sizeof(double))); + double* x = static_cast(malloc(Nmeas * sizeof(double))); + double* p = static_cast(malloc(Nstate * sizeof(double))); + ASSERT(x0); + ASSERT(x); + ASSERT(p); + + memcpy(p, p0, Nstate * sizeof(double)); + + + cholmod_common _cholmod_common; + cholmod_sparse* Jt; + cholmod_sparse* Jt0; + double* J_dense = NULL; // setting to NULL to pacify compiler's "uninitialized" warnings + double* J_dense0 = NULL; // setting to NULL to pacify compiler's "uninitialized" warnings + + + // This is a plain text table, that can be easily parsed with "vnlog" tools + printf("# ivar imeasurement gradient_reported gradient_observed error error_relative\n"); + + if( is_sparse ) + { + if( !cholmod_start(&_cholmod_common) ) + { + SAY( "Couldn't initialize CHOLMOD"); + return; + } + + Jt = cholmod_allocate_sparse(Nstate, Nmeas, NJnnz, + 1, // sorted + 1, // packed, + 0, // NOT symmetric + CHOLMOD_REAL, + &_cholmod_common); + Jt0 = cholmod_allocate_sparse(Nstate, Nmeas, NJnnz, + 1, // sorted + 1, // packed, + 0, // NOT symmetric + CHOLMOD_REAL, + &_cholmod_common); + + p[var] -= GRADTEST_DELTA/2.0; + (*f)(p, x0, Jt0, cookie); + p[var] += GRADTEST_DELTA; + (*f)(p, x, Jt, cookie); + p[var] -= GRADTEST_DELTA/2.0; + } + else + { + J_dense = static_cast(malloc( Nmeas * Nstate * sizeof(J_dense[0]) )); + J_dense0 = static_cast(malloc( Nmeas * Nstate * sizeof(J_dense[0]) )); + + dogleg_callback_dense_t* f_dense = (dogleg_callback_dense_t*)f; + p[var] -= GRADTEST_DELTA/2.0; + (*f_dense)(p, x0, J_dense0, cookie); + p[var] += GRADTEST_DELTA; + (*f_dense)(p, x, J_dense, cookie); + p[var] -= GRADTEST_DELTA/2.0; + } + + + for(unsigned int i=0; i 0, instead I have %d", NJnnz); + return; + } + return _dogleg_testGradient(var, p0, Nstate, Nmeas, NJnnz, f, cookie); +} +void dogleg_testGradient_dense(unsigned int var, const double* p0, + unsigned int Nstate, unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie) +{ + return _dogleg_testGradient(var, p0, Nstate, Nmeas, 0, (dogleg_callback_t*)f, cookie); +} + + +////////////////////////////////////////////////////////////////////////////////////////// +// solver routines +////////////////////////////////////////////////////////////////////////////////////////// + +static void computeCauchyUpdate(dogleg_operatingPoint_t* point, + const dogleg_solverContext_t* ctx) +{ + // I already have this data, so don't need to recompute + if(!point->updateCauchy_valid) + { + point->updateCauchy_valid = 1; + + // I look at a step in the steepest direction that minimizes my + // quadratic error function (Cauchy point). If this is past my trust region, + // I move as far as the trust region allows along the steepest descent + // direction. My error function is F=norm2(f(p)). dF/dP = 2*ft*J. + // This is proportional to Jt_x, which is thus the steepest ascent direction. + // + // Thus along this direction we have F(k) = norm2(f(p + k Jt_x)). The Cauchy + // point is where F(k) is at a minimum: + // dF_dk = 2 f(p + k Jt_x)t J Jt_x ~ (x + k J Jt_x)t J Jt_x = + // = xt J Jt x + k xt J Jt J Jt x = norm2(Jt x) + k norm2(J Jt x) + // dF_dk = 0 -> k= -norm2(Jt x) / norm2(J Jt x) + // Summary: + // the steepest direction is parallel to Jt*x. The Cauchy point is at + // k*Jt*x where k = -norm2(Jt*x)/norm2(J*Jt*x) + double norm2_Jt_x = norm2(point->Jt_x, ctx->Nstate); + double norm2_J_Jt_x = ctx->is_sparse ? + norm2_mul_spmatrix_t_densevector(point->Jt, point->Jt_x) : + norm2_mul_matrix_vector (point->J_dense, point->Jt_x, ctx->Nmeasurements, ctx->Nstate); + double k = -norm2_Jt_x / norm2_J_Jt_x; + + point->updateCauchy_lensq = k*k * norm2_Jt_x; + + vec_copy_scaled(point->updateCauchy, + point->Jt_x, k, ctx->Nstate); + SAY_IF_VERBOSE( "cauchy step size %.6g", sqrt(point->updateCauchy_lensq)); + } + + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_data.step_len_cauchy = sqrt(point->updateCauchy_lensq); +} + +// LAPACK prototypes for a packed cholesky factorization and a linear solve +// using that factorization, respectively +extern "C" { +int dpptrf_(char* uplo, int* n, double* ap, + int* info, int uplo_len); +int dpptrs_(char* uplo, int* n, int* nrhs, + double* ap, double* b, int* ldb, int* info, + int uplo_len); +} + + +void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx) +{ + // I already have this data, so don't need to recompute + if(point->updateGN_valid) + return; + + if( ctx->is_sparse ) + { + // I'm assuming the pattern of zeros will remain the same throughout, so I + // analyze only once + if(ctx->factorization == NULL) + { + ctx->factorization = cholmod_analyze(point->Jt, &ctx->common); + ASSERT(ctx->factorization != NULL); + } + + while(1) + { + if( ctx->lambda == 0.0 ) + ASSERT( cholmod_factorize(point->Jt, ctx->factorization, &ctx->common) ); + else + { + double beta[] = { ctx->lambda, 0 }; + ASSERT( cholmod_factorize_p(point->Jt, beta, NULL, 0, + ctx->factorization, &ctx->common) ); + } + + if(ctx->factorization->minor == ctx->factorization->n) + break; + + // singular JtJ. Raise lambda and go again + if( ctx->lambda == 0.0) ctx->lambda = LAMBDA_INITIAL; + else ctx->lambda *= 10.0; + ASSERT( isfinite(ctx->lambda) ); + + SAY_IF_VERBOSE( "singular JtJ. Have rank/full rank: %zd/%d. Adding %g I from now on", + ctx->factorization->minor, ctx->Nstate, ctx->lambda); + } + } + else + { + if(ctx->factorization_dense == NULL) + { + // Need to store symmetric JtJ, so I only need one triangle of it + ctx->factorization_dense = static_cast(malloc( ctx->Nstate * (ctx->Nstate+1) / 2 * + sizeof( ctx->factorization_dense[0]))); + ASSERT(ctx->factorization_dense); + } + + while(1) + { + // I construct my JtJ. JtJ is packed and stored row-first. I have two + // equivalent implementations. The one enabled here is maybe a bit faster, + // but it's definitely clearer +#if 1 + memset(ctx->factorization_dense, + 0, + ctx->Nstate*(ctx->Nstate+1)/2*sizeof(ctx->factorization_dense[0])); + for(int i=0; iNmeasurements; i++) + accum_outerproduct_packed( ctx->factorization_dense, &point->J_dense[ctx->Nstate*i], + ctx->Nstate ); + if( ctx->lambda > 0.0 ) + { + int iJtJ=0; + for(int i1=0; i1Nstate; i1++) + { + ctx->factorization_dense[iJtJ] += ctx->lambda; + iJtJ += ctx->Nstate-i1; + } + } +#else + int iJtJ = 0; + for(int i1=0; i1Nstate; i1++) + { + #error this does not work. overwritten in the following loop + ctx->factorization_dense[iJtJ] += ctx->lambda; + + for(int i0=i1; i0Nstate; i0++, iJtJ++) + ctx->factorization_dense[iJtJ] = inner_withstride( &point->J_dense[i0], + &point->J_dense[i1], + ctx->Nmeasurements, + ctx->Nstate); + } +#endif + + + + int info; + char uplo = 'L'; + int Nstate_copy = ctx->Nstate; + dpptrf_(&uplo, &Nstate_copy, ctx->factorization_dense, + &info, 1); + ASSERT(info >= 0); // we MUST either succeed or see complain of singular + // JtJ + if( info == 0 ) + break; + + // singular JtJ. Raise lambda and go again + if( ctx->lambda == 0.0) ctx->lambda = LAMBDA_INITIAL; + else ctx->lambda *= 10.0; + + SAY_IF_VERBOSE( "singular JtJ. Adding %g I from now on", ctx->lambda); + } + } +} + +static void computeGaussNewtonUpdate(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx) +{ + // I already have this data, so don't need to recompute + if(!point->updateGN_valid) + { + dogleg_computeJtJfactorization(point, ctx); + + // try to factorize the matrix directly. If it's singular, add a small + // constant to the diagonal. This constant gets larger if we keep being + // singular + if( ctx->is_sparse ) + { + // solve JtJ*updateGN = Jt*x. Gauss-Newton step is then -updateGN + cholmod_dense Jt_x_dense = {.nrow = static_cast(ctx->Nstate), + .ncol = 1, + .nzmax = static_cast(ctx->Nstate), + .d = static_cast(ctx->Nstate), + .x = point->Jt_x, + .xtype = CHOLMOD_REAL, + .dtype = CHOLMOD_DOUBLE}; + + if(point->updateGN_cholmoddense != NULL) + cholmod_free_dense(&point->updateGN_cholmoddense, &ctx->common); + + point->updateGN_cholmoddense = cholmod_solve(CHOLMOD_A, + ctx->factorization, + &Jt_x_dense, + &ctx->common); + vec_negate(static_cast(point->updateGN_cholmoddense->x), + ctx->Nstate); // should be more efficient than this later + + point->updateGN_lensq = norm2(static_cast(point->updateGN_cholmoddense->x), ctx->Nstate); + } + else + { + memcpy( point->updateGN_dense, point->Jt_x, ctx->Nstate * sizeof(point->updateGN_dense[0])); + int info; + char uplo = 'L'; + int nhrs = 1; + int Nstate_copy = ctx->Nstate; + int Nstate_copy2 = ctx->Nstate; + dpptrs_(&uplo, &Nstate_copy, &nhrs, + ctx->factorization_dense, + point->updateGN_dense, &Nstate_copy2, &info, 1); + vec_negate(point->updateGN_dense, + ctx->Nstate); // should be more efficient than this later + + point->updateGN_lensq = norm2(point->updateGN_dense, ctx->Nstate); + } + + SAY_IF_VERBOSE( "gn step size %.6g", sqrt(point->updateGN_lensq)); + point->updateGN_valid = 1; + } + + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_data.step_len_gauss_newton = sqrt(point->updateGN_lensq); + +} + +static void computeInterpolatedUpdate(double* update_dogleg, + double* update_dogleg_lensq, + dogleg_operatingPoint_t* point, + double trustregion, + const dogleg_solverContext_t* ctx) +{ + // I interpolate between the Cauchy-point step and the Gauss-Newton step + // to find a step that takes me to the edge of my trust region. + // + // I have something like norm2(a + k*(b-a)) = dsq + // = norm2(a) + 2*at*(b-a) * k + norm2(b-a)*k^2 = dsq + // let c = at*(b-a), l2 = norm2(b-a) -> + // l2 k^2 + 2*c k + norm2(a)-dsq = 0 + // + // This is a simple quadratic equation: + // k = (-2*c +- sqrt(4*c*c - 4*l2*(norm2(a)-dsq)))/(2*l2) + // = (-c +- sqrt(c*c - l2*(norm2(a)-dsq)))/l2 + + // to make 100% sure the discriminant is positive, I choose a to be the + // cauchy step. The solution must have k in [0,1], so I much have the + // +sqrt side, since the other one is negative + double dsq = trustregion*trustregion; + double norm2a = point->updateCauchy_lensq; + const double* a = point->updateCauchy; + const double* b = ctx->is_sparse ? static_cast(point->updateGN_cholmoddense->x) : point->updateGN_dense; + + double l2 = 0.0; + double neg_c = 0.0; + for(int i=0; iNstate; i++) + { + double d = a[i] - b[i]; + + l2 += d*d; + neg_c += d*a[i]; + } + + double discriminant = neg_c*neg_c - l2* (norm2a - dsq); + if(discriminant < 0.0) + { + SAY( "negative discriminant: %.6g!", discriminant); + discriminant = 0.0; + } + double k = (neg_c + sqrt(discriminant))/l2; + + *update_dogleg_lensq = 0.0; + for(int i=0; iNstate; i++) + { + update_dogleg[i] = a[i] + k*(b[i] - a[i]); + *update_dogleg_lensq += update_dogleg[i]*update_dogleg[i]; + } + + SAY_IF_VERBOSE( "k_cauchy_to_gn %.6g, norm %.6g", + k, + sqrt(*update_dogleg_lensq)); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.step_len_interpolated = sqrt(*update_dogleg_lensq); + vnlog_debug_data.k_cauchy_to_gn = k; + } +} + +// takes in point->p, and computes all the quantities derived from it, storing +// the result in the other members of the operatingPoint structure. Returns +// true if the gradient-size termination criterion has been met +static int computeCallbackOperatingPoint(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx) +{ + if( ctx->is_sparse ) + { + (*ctx->f)(point->p, point->x, point->Jt, ctx->cookie); + + // compute Jt*x + mul_spmatrix_densevector(point->Jt_x, point->Jt, point->x); + } + else + { + (*ctx->f_dense)(point->p, point->x, point->J_dense, ctx->cookie); + + // compute Jt*x + mul_matrix_t_densevector(point->Jt_x, point->J_dense, point->x, + ctx->Nmeasurements, ctx->Nstate); + } + + // I just got a new operating point, so the current update vectors aren't + // valid anymore, and should be recomputed, as needed + point->updateCauchy_valid = 0; + point->updateGN_valid = 0; + + // compute the 2-norm of the current error vector + // At some point this should be changed to use the call from libminimath + point->norm2_x = norm2(point->x, ctx->Nmeasurements); + + // If the largest absolute gradient element is smaller than the threshold, + // we can stop iterating. This is equivalent to the inf-norm + for(int i=0; iNstate; i++) + if(fabs(point->Jt_x[i]) > ctx->parameters->Jt_x_threshold) + return 0; + SAY_IF_VERBOSE( "Jt_x all below the threshold. Done iterating!"); + + return 1; +} +static double computeExpectedImprovement(const double* step, const dogleg_operatingPoint_t* point, + const dogleg_solverContext_t* ctx) +{ + // My error function is F=norm2(f(p + step)). F(0) - F(step) = + // = norm2(x) - norm2(x + J*step) = -2*inner(x,J*step) - norm2(J*step) + // = -2*inner(Jt_x,step) - norm2(J*step) + if( ctx->is_sparse ) + return + - 2.0*inner(point->Jt_x, step, ctx->Nstate) + - norm2_mul_spmatrix_t_densevector(point->Jt, step); + else + return + - 2.0*inner(point->Jt_x, step, ctx->Nstate) + - norm2_mul_matrix_vector(point->J_dense, step, ctx->Nmeasurements, ctx->Nstate); +} + + +// takes a step from the given operating point, using the given trust region +// radius. Returns the expected improvement, based on the step taken and the +// linearized x(p). If we can stop iterating, returns a negative number +static double takeStepFrom(dogleg_operatingPoint_t* pointFrom, + double* p_new, + double* step, + double* step_len_sq, + double trustregion, + dogleg_solverContext_t* ctx) +{ + SAY_IF_VERBOSE( "taking step with trustregion %.6g", trustregion); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.trustregion_before = trustregion; + vnlog_debug_data.norm2x_before = pointFrom->norm2_x; + } + + computeCauchyUpdate(pointFrom, ctx); + + if(pointFrom->updateCauchy_lensq >= trustregion*trustregion) + { + SAY_IF_VERBOSE( "taking cauchy step"); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.step_type = STEPTYPE_CAUCHY; + vnlog_debug_data.step_len = vnlog_debug_data.step_len_cauchy; + } + *step_len_sq = pointFrom->updateCauchy_lensq; + + // cauchy step goes beyond my trust region, so I do a gradient descent + // to the edge of my trust region and call it good + vec_copy_scaled(step, + pointFrom->updateCauchy, + trustregion / sqrt(pointFrom->updateCauchy_lensq), + ctx->Nstate); + pointFrom->didStepToEdgeOfTrustRegion = 1; + } + else + { + // I'm not yet done. The cauchy point is within the trust region, so I can + // go further. I look at the full Gauss-Newton step. If this is within the + // trust region, I use it. Otherwise, I find the point at the edge of my + // trust region that lies on a straight line between the Cauchy point and + // the Gauss-Newton solution, and use that. This is the heart of Powell's + // dog-leg algorithm. + computeGaussNewtonUpdate(pointFrom, ctx); + if(pointFrom->updateGN_lensq <= trustregion*trustregion) + { + SAY_IF_VERBOSE( "taking GN step"); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.step_type = STEPTYPE_GAUSSNEWTON; + vnlog_debug_data.step_len = vnlog_debug_data.step_len_gauss_newton; + } + *step_len_sq = pointFrom->updateGN_lensq; + + // full Gauss-Newton step lies within my trust region. Take the full step + memcpy( step, + ctx->is_sparse ? pointFrom->updateGN_cholmoddense->x : pointFrom->updateGN_dense, + ctx->Nstate * sizeof(step[0]) ); + pointFrom->didStepToEdgeOfTrustRegion = 0; + } + else + { + SAY_IF_VERBOSE( "taking interpolated step"); + + // full Gauss-Newton step lies outside my trust region, so I interpolate + // between the Cauchy-point step and the Gauss-Newton step to find a step + // that takes me to the edge of my trust region. + computeInterpolatedUpdate(step, + step_len_sq, + pointFrom, trustregion, ctx); + pointFrom->didStepToEdgeOfTrustRegion = 1; + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.step_type = STEPTYPE_INTERPOLATED; + vnlog_debug_data.step_len = vnlog_debug_data.step_len_interpolated; + } + } + } + + // take the step + vec_add(p_new, pointFrom->p, step, ctx->Nstate); + double expectedImprovement = computeExpectedImprovement(step, pointFrom, ctx); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.expected_improvement = expectedImprovement; + + if(pointFrom->step_to_here_len_sq != INFINITY) + { + double cos_direction_change = + inner(step, pointFrom->step_to_here, ctx->Nstate) / + sqrt(*step_len_sq * pointFrom->step_to_here_len_sq); + + // check the numerical overflow cases + if(cos_direction_change >= 1.0) + vnlog_debug_data.step_direction_change_deg = 0.0; + else if(cos_direction_change <= -1.0) + vnlog_debug_data.step_direction_change_deg = 180.0; + else + vnlog_debug_data.step_direction_change_deg = 180.0/M_PI*acos(cos_direction_change); + } + } + + // are we done? For each state variable I look at the update step. If all the elements fall below + // a threshold, I call myself done + for(int i=0; iNstate; i++) + if( fabs(step[i]) > ctx->parameters->update_threshold ) + return expectedImprovement; + + SAY_IF_VERBOSE( "update small enough. Done iterating!"); + + return -1.0; +} + + +// I have a candidate step. I adjust the trustregion accordingly, and also +// report whether this step should be accepted (0 == rejected, otherwise +// accepted) +static int evaluateStep_adjustTrustRegion(const dogleg_operatingPoint_t* before, + const dogleg_operatingPoint_t* after, + double* trustregion, + double expectedImprovement, + dogleg_solverContext_t* ctx) +{ + double observedImprovement = before->norm2_x - after->norm2_x; + double rho = observedImprovement / expectedImprovement; + SAY_IF_VERBOSE( "observed/expected improvement: %.6g/%.6g. rho = %.6g", + observedImprovement, expectedImprovement, rho); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + { + vnlog_debug_data.observed_improvement = observedImprovement; + vnlog_debug_data.rho = rho; + } + + + // adjust the trust region + if( rho < ctx->parameters->trustregion_decrease_threshold ) + { + SAY_IF_VERBOSE( "rho too small. decreasing trust region"); + + // Our model doesn't fit well. We should reduce the trust region size. If + // the trust region size was affecting the attempted step, do this by a + // constant factor. Otherwise, drop the trustregion to attempted step size + // first + if( !before->didStepToEdgeOfTrustRegion ) + *trustregion = sqrt(before->updateGN_lensq); + + *trustregion *= ctx->parameters->trustregion_decrease_factor; + } + else if (rho > ctx->parameters->trustregion_increase_threshold && before->didStepToEdgeOfTrustRegion) + { + SAY_IF_VERBOSE( "rho large enough. increasing trust region"); + + *trustregion *= ctx->parameters->trustregion_increase_factor; + } + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + vnlog_debug_data.trustregion_after = *trustregion; + + return rho > 0.0; +} + +static int runOptimizer(dogleg_solverContext_t* ctx) +{ + double trustregion = ctx->parameters->trustregion0; + int stepCount = 0; + + if( computeCallbackOperatingPoint(ctx->beforeStep, ctx) ) + return stepCount; + + SAY_IF_VERBOSE( "Initial operating point has norm2_x %.6g", ctx->beforeStep->norm2_x); + + + ctx->beforeStep->step_to_here_len_sq = INFINITY; + + while( stepCountparameters->max_iterations ) + { + SAY_IF_VERBOSE( "================= step %d", stepCount ); + + while(1) + { + SAY_IF_VERBOSE("--------"); + + double expectedImprovement = + takeStepFrom(ctx->beforeStep, + ctx->afterStep->p, + ctx->afterStep->step_to_here, + &ctx->afterStep->step_to_here_len_sq, + trustregion, ctx); + + // negative expectedImprovement is used to indicate that we're done + if(expectedImprovement < 0.0) + { + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_emit_record(stepCount, 1); + return stepCount; + } + + int afterStepZeroGradient = computeCallbackOperatingPoint(ctx->afterStep, ctx); + SAY_IF_VERBOSE( "Evaluated operating point with norm2_x %.6g", ctx->afterStep->norm2_x); + if(ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG) + vnlog_debug_data.norm2x_after = ctx->afterStep->norm2_x; + + if( evaluateStep_adjustTrustRegion(ctx->beforeStep, ctx->afterStep, &trustregion, + expectedImprovement, ctx) ) + { + SAY_IF_VERBOSE( "accepted step"); + + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_emit_record(stepCount, 1); + stepCount++; + + // I accept this step, so the after-step operating point is the before-step operating point + // of the next iteration. I exchange the before- and after-step structures so that all the + // pointers are still around and I don't have to re-allocate + dogleg_operatingPoint_t* tmp; + tmp = ctx->afterStep; + ctx->afterStep = ctx->beforeStep; + ctx->beforeStep = tmp; + + if( afterStepZeroGradient ) + { + SAY_IF_VERBOSE( "Gradient low enough and we just improved. Done iterating!" ); + + return stepCount; + } + + break; + } + + SAY_IF_VERBOSE( "rejected step"); + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_emit_record(stepCount, 0); + + // This step was rejected. check if the new trust region size is small + // enough to give up + if(trustregion < ctx->parameters->trustregion_threshold) + { + SAY_IF_VERBOSE( "trust region small enough. Giving up. Done iterating!"); + return stepCount; + } + + // I have rejected this step, so I try again with the new trust region + } + } + + if(stepCount == ctx->parameters->max_iterations) + SAY_IF_VERBOSE( "Exceeded max number of iterations"); + + return stepCount; +} + +static +dogleg_operatingPoint_t* allocOperatingPoint(int Nstate, int numMeasurements, + unsigned int NJnnz, + cholmod_common* common) +{ + int is_sparse = NJnnz > 0; + + dogleg_operatingPoint_t* point = static_cast(malloc(sizeof(dogleg_operatingPoint_t))); + ASSERT(point != NULL); + + + int Npool = + Nstate + + numMeasurements + + Nstate + + Nstate + + Nstate; + if(!is_sparse) + Npool += numMeasurements*Nstate + Nstate; + + double* pool = static_cast(malloc( Npool * sizeof(double) )); + ASSERT( pool != NULL ); + + point->p = &pool[0]; + point->x = &pool[Nstate]; + point->Jt_x = &pool[Nstate + + numMeasurements]; + point->updateCauchy = &pool[Nstate + + numMeasurements + + Nstate]; + point->step_to_here = &pool[Nstate + + numMeasurements + + Nstate + + Nstate]; + + if( is_sparse ) + { + point->Jt = cholmod_allocate_sparse(Nstate, numMeasurements, NJnnz, + 1, // sorted + 1, // packed, + 0, // NOT symmetric + CHOLMOD_REAL, + common); + ASSERT(point->Jt != NULL); + point->updateGN_cholmoddense = NULL; // This will be allocated as it is used + } + else + { + point->J_dense = &pool[Nstate + + numMeasurements + + Nstate + + Nstate + + Nstate]; + point->updateGN_dense = &pool[Nstate + + numMeasurements + + Nstate + + Nstate + + Nstate + + numMeasurements * Nstate]; + } + + // vectors don't have any valid data yet + point->updateCauchy_valid = 0; + point->updateGN_valid = 0; + + return point; +} + +static void freeOperatingPoint(dogleg_operatingPoint_t** point, cholmod_common* common) +{ + // MUST match allocOperatingPoint(). It does a single malloc() and stores the + // pointer into p + free((*point)->p); + + int is_sparse = common != NULL; + + if( is_sparse ) + { + cholmod_free_sparse(&(*point)->Jt, common); + + if((*point)->updateGN_cholmoddense != NULL) + cholmod_free_dense(&(*point)->updateGN_cholmoddense, common); + } + + free(*point); + *point = NULL; +} + +static int cholmod_error_callback(const char* s, ...) +{ + SAY_NONEWLINE(""); + + va_list ap; + va_start(ap, s); + int ret = vfprintf(stderr, s, ap); + va_end(ap); + fprintf(stderr, "\n"); + return ret; +} + +static void set_cholmod_options(cholmod_common* cc) +{ + // I want to use LGPL parts of CHOLMOD only, so I turn off the supernodal routines. This gave me a + // 25% performance hit in the solver for a particular set of optical calibration data. + cc->supernodal = 0; + + + // I want all output to go to STDERR, not STDOUT +#if (CHOLMOD_VERSION <= (CHOLMOD_VER_CODE(2,2))) + cc->print_function = cholmod_error_callback; +#elif (CHOLMOD_VERSION < (CHOLMOD_VER_CODE(4,0))) + CHOLMOD_FUNCTION_DEFAULTS ; + CHOLMOD_FUNCTION_PRINTF(cc) = cholmod_error_callback; +#else + SuiteSparse_config_printf_func_set(cholmod_error_callback); +#endif +} + +void dogleg_freeContext(dogleg_solverContext_t** ctx) +{ + freeOperatingPoint(&(*ctx)->beforeStep, (*ctx)->is_sparse ? &(*ctx)->common : NULL); + freeOperatingPoint(&(*ctx)->afterStep, (*ctx)->is_sparse ? &(*ctx)->common : NULL); + + if( (*ctx)->is_sparse ) + { + if((*ctx)->factorization != NULL) + cholmod_free_factor (&(*ctx)->factorization, &(*ctx)->common); + cholmod_finish(&(*ctx)->common); + } + else + free((*ctx)->factorization_dense); + + free(*ctx); + *ctx = NULL; +} + +static double _dogleg_optimize(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, + void* cookie, + + // NULL to use the globals + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext) +{ + int is_sparse = NJnnz > 0; + + + dogleg_solverContext_t* ctx = static_cast(malloc(sizeof(dogleg_solverContext_t))); + ctx->f = f; + ctx->cookie = cookie; + ctx->factorization = NULL; + ctx->lambda = 0.0; + ctx->Nstate = Nstate; + ctx->Nmeasurements = Nmeas; + ctx->parameters = parameters ? parameters : ¶meters_global; + + if( ctx->parameters->dogleg_debug & DOGLEG_DEBUG_VNLOG ) + vnlog_debug_emit_legend(); + + if( returnContext != NULL ) + *returnContext = ctx; + + if( is_sparse ) + { + if( !cholmod_start(&ctx->common) ) + { + SAY( "Couldn't initialize CHOLMOD"); + return -1.0; + } + set_cholmod_options(&ctx->common); + ctx->is_sparse = 1; + } + else + ctx->is_sparse = 0; + + ctx->beforeStep = allocOperatingPoint(Nstate, Nmeas, NJnnz, &ctx->common); + ctx->afterStep = allocOperatingPoint(Nstate, Nmeas, NJnnz, &ctx->common); + + memcpy(ctx->beforeStep->p, p, Nstate * sizeof(double)); + + // everything is set up, so run the solver! + int numsteps = runOptimizer(ctx); + double norm2_x = ctx->beforeStep->norm2_x; + + // runOptimizer places the most recent results into beforeStep in preparation for another + // iteration + memcpy(p, ctx->beforeStep->p, Nstate * sizeof(double)); + + SAY_IF_VERBOSE( "success! took %d iterations", numsteps); + + if( returnContext == NULL ) + dogleg_freeContext(&ctx); + + return norm2_x; +} + +double dogleg_optimize2(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, + void* cookie, + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext) +{ + if( NJnnz == 0 ) + { + SAY( "I must have NJnnz > 0, instead I have %d", NJnnz); + return -1.0; + } + + return _dogleg_optimize(p, Nstate, Nmeas, NJnnz, f, + cookie, + parameters, + returnContext); +} + +double dogleg_optimize(double* p, unsigned int Nstate, + unsigned int Nmeas, unsigned int NJnnz, + dogleg_callback_t* f, + void* cookie, + dogleg_solverContext_t** returnContext) +{ + return dogleg_optimize2(p,Nstate,Nmeas,NJnnz,f,cookie, + NULL, // no parameters; use the globals + returnContext); +} + + +double dogleg_optimize_dense2(double* p, unsigned int Nstate, + unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie, + const dogleg_parameters2_t* parameters, + dogleg_solverContext_t** returnContext) +{ + return _dogleg_optimize(p, Nstate, Nmeas, 0, (dogleg_callback_t*)f, + cookie, + parameters, + returnContext); +} +double dogleg_optimize_dense(double* p, unsigned int Nstate, + unsigned int Nmeas, + dogleg_callback_dense_t* f, void* cookie, + dogleg_solverContext_t** returnContext) +{ + return dogleg_optimize_dense2(p,Nstate,Nmeas,f,cookie, + NULL, // no parameters; use the globals + returnContext); +} + + + + + + + +// Computes pinv(J) for a subset of measurements: inv(JtJ) * +// Jt[imeasurement0..imeasurement0+N-1]. Returns false if something failed. +// ASSUMES THAT THE CHOLESKY FACTORIZATION HAS ALREADY BEEN COMPUTED. +// +// This function is experimental, and subject to change +static bool pseudoinverse_J_dense(// output + double* out, + + // inputs + const dogleg_operatingPoint_t* point, + const dogleg_solverContext_t* ctx, + int i_meas0, int NmeasInChunk) +{ + int info; + memcpy(out, + &point->J_dense[i_meas0*ctx->Nstate], + NmeasInChunk*ctx->Nstate*sizeof(double)); + char uplo = 'L'; + int Nstate_copy = ctx->Nstate; + int Nstate_copy2 = ctx->Nstate; + dpptrs_(&uplo, &Nstate_copy, &NmeasInChunk, + ctx->factorization_dense, + out, + &Nstate_copy2, &info, 1); + return info==0; +} + +// Computes pinv(J) for a subset of measurements: inv(JtJ) * +// Jt[imeasurement0..imeasurement0+N-1]. Returns false if something failed. +// ASSUMES THAT THE CHOLESKY FACTORIZATION HAS ALREADY BEEN COMPUTED. +// +// allocates memory, returns NULL on failure. ON SUCCESS, THE CALLER IS +// RESPONSIBLE FOR FREEING THE RETURNED MEMORY +// +// This function is experimental, and subject to change +static cholmod_dense* pseudoinverse_J_sparse(// inputs + const dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx, + int i_meas0, int NmeasInChunk, + + // Pre-allocated array for the + // right-hand-side. This will be used as a + // workspace. Create this like so: + // + // cholmod_allocate_dense( Nstate, + // NmeasInChunk, + // Nstate, + // CHOLMOD_REAL, + // &ctx->common ); + + cholmod_dense* Jt_chunk) +{ + // I'm solving JtJ x = b where J is sparse, b is sparse, but x ends up dense. + // cholmod doesn't have functions for this exact case. so I use the + // dense-sparse-dense function (cholmod_solve), and densify the input. Instead + // of sparse-sparse-sparse and the densifying the output (cholmod_spsolve). + // This feels like it'd be more efficient + + memset( Jt_chunk->x, 0, Jt_chunk->nrow*Jt_chunk->ncol*sizeof(double) ); + int Jt_chunk_ncol_backup = Jt_chunk->ncol; + for(int i_meas=0; i_meas= (int)ctx->Nmeasurements ) + { + // at the end, we could have one chunk with less that chunk_size + // columns + Jt_chunk->ncol = i_meas; + break; + } + + for(unsigned int i0=P(point->Jt, i_meas0+i_meas); i0Jt, i_meas0+i_meas+1); i0++) + { + int irow = I(point->Jt,i0); + double x = X(point->Jt,i0); + ((double*)Jt_chunk->x)[irow + i_meas*Jt_chunk->nrow] = x; + } + } + + // solve inv(JtJ)Jt[slice] + cholmod_dense* pinv = + cholmod_solve(CHOLMOD_A, + ctx->factorization, + Jt_chunk, + &ctx->common); + Jt_chunk->ncol = Jt_chunk_ncol_backup; + return pinv; +} + + +/* +The below is a bunch of code for outlier detection/rejection and for confidence +evaluation. IT'S ALL EXPERIMENTAL AND SUBJECT TO CHANGE + +What is an outlier? Suppose I just found an optimum. I define an outlier as an +observation that does two things to the problem if I remove that observation: + +1. The cost function would improve significantly. Things would clearly improve + because the cost function contribution of the removed point itself would get + removed, but ALSO because the parameters could fit the remaining data better + without that extra observation; the "outlier" wouldn't pull the solution away + from the "true" optimum. + +2. The confidence of the solution does not significantly decrease. One could + imagine a set of data that define the problem poorly, and produce a low cost + function value for some (overfit) set of parameters. And one can imagine an + extra point being added that defines the problem and increases the confidence + of the solution. This extra point would suppress the overfitting, so this + extra point would increase the cost function value. Condition 1 above would + make this extra point look like an outlier, and this condition is meant to + detect this case and to classify this point as NOT an outlier + +Let's say we just computed an optimum least-squares fit, and we try to determine +if some of the data points look like outliers. The least squares problem I just +solved has cost function + + E = norm2(x) + +where x is a length-N vector of measurements. We solved it by optimizing the +vector of parameters p. We're at an optimum, so dE/dp = 0 -> Jt x = 0 + +We define an outlier as a measurement that would greatly improve the cost +function E if this measurement was removed, and the problem was re-optimized. + +Let's say the problem is locally linear (J = dx/dp is constant), and let's say +re-optimizing moves the parameter by dp. The original cost function was + + E0 = norm2(x) + +If we move by dp, and take away the outlier's own cost, we get + + E1 = norm2(x + J dp) - norm2( x* + J* dp ) + +where x* and J* refer to the outlier measurements. Thus "Dima's self+others +factor" is norm2(x) - (norm2(x + J dp) - norm2( x* + J* dp )) + +We can choose to look at ONLY the effect on the other variables. That would +produce "Dima's others factor" = norm2(x)-norm2(x*) - (norm2(x + J dp) - +norm2(x* + J* dp )) + +This is very similar to Cook's D factor (which Dima apparently reinvented 40 +years after the fact!). This factor looks not at the difference of norm2(x), but +at norm2(difference) instead. So "Cook's self+others factor" is proportional to +norm2(x - (x+J dp)). This is the "normal" Cook's D factor. We can also compute +"Cook's others factor": norm2(x - vec(x*) - (x + J dp -(vec(x*) + full(J*) dp))) += norm2( - J dp + full(J*) dp) = norm2(J dp) + norm2(full(J*) dp) -2 dpt(Jt +full(J*) + full(J*)tJ)dp = norm2(J dp) + norm2(full(J*) dp) -2 dpt(Jt full(J*) + +full(J*)tJ)dp = norm2(J dp) + norm2(J* dp) - 2 norm2(J* dp) = norm2(J dp) - +norm2(J* dp) + +This is 4 flavors of a very similar computation. In summary (and ignoring scale +factors): + + Dima's self+others: -norm2(J dp) + 2 x*t J* dp + norm2(J* dp) + norm2(x*) + Dima's others : -norm2(J dp) + 2 x*t J* dp + norm2(J* dp) + Cook's self+others: norm2(J dp) + Cook's others : norm2(J dp) - norm2(J* dp) + +Let's compute these. dE1/dp = 0 at p+dp -> + + 0 = Jt x + JtJ dp - J*t x* - J*tJ* dp + = JtJ dp - J*t x* - J*tJ* dp + +-> dp = inv(JtJ - J*tJ*) J*t x* + +Woodbury identity: + + inv(JtJ - J*t J*) = + = inv(JtJ) - inv(JtJ) J*t inv(-I + J* inv(JtJ) J*t) J* inv(JtJ) + +Let + A = J* inv(JtJ) J*t + B = inv(A - I) + +So + AB = BA = I+B + +Thus + inv(JtJ - J*t J*) = + = inv(JtJ) - inv(JtJ) J*t B J* inv(JtJ) + +and + + dp = inv(JtJ - J*tJ*) J*t x* = + = inv(JtJ)J*t x* - inv(JtJ) J*t B J* inv(JtJ) J*t x* + = inv(JtJ)J*t x* - inv(JtJ) J*t B A x* + = inv(JtJ)J*t(I - B A) x* + = -inv(JtJ)J*t B x* + +Then + + norm2(J dp) = x*t ( B J* inv() Jt J inv() J*t B ) x* + = x*t ( B J* inv() J*t B ) x* + = x*t ( B A B ) x* + = x*t ( B + B*B ) x* + + 2 x*t J* dp = -2 x*t J* inv(JtJ)J*t B x* = + = -2 x*t A B x* = + = x*t (-2AB) x* = + = x*t (-2I - 2B) x* + + norm2(J* dp) = x*t ( B J* inv() J*tJ* inv() J*t B ) x* = + = x*t ( B A A B ) x* = + = x*t ( I + 2B + B*B ) x* + + norm2(x*) = x*t ( I ) x* + +There're two ways to compute the "Dima's self-only" factor. I can simply say +that the measurements' cost of norm2(x*) has been removed, so the factor is x*t +I x* or I can + +1. Remove measurements +2. Re-optimize +3. Look to see how the residual of x* changes + +This is different because I look at what happens to x*(p) when x* is no longer +in the optimized set. + +State moves by dp. x* moves by J* dp. I look at + + dE = norm2(x* + J* dp) - norm2(x*) + = 2 x*t J* dp + norm2(J* dp) = + = x*t (-2I - 2B + I + 2B + B*B ) x* = + = x*t (B*B - I) x* + +I expect that removing measurements from the optimization should make their +residuals worse. I.e. I expect dE > 0. Let's check. Is B*B - I positive +definite? Let's say that there's v,l such that + + (B*B-I)v = l v, norm2(v) = 1 + --> + BBv = (l+1)v + vBBv = l+1 + + Let u = Bv -> + norm2(u) = l+1 + + A = J* inv(JtJ) J*t + B = inv(A - I) -> + + v = (A-I)u + norm2(v) = 1 = norm2(Au) - 2ut A u + norm2(u) -> + -> l = 2ut A u - norm2(Au) + + Let w = J*t u + -> Au = J* inv(JtJ) w + -> ut A u = wt inv(JtJ) w -> + l = wt ( 2inv(JtJ) - inv(JtJ)J*tJ* inv(JtJ) ) w + + J*t J* = sum(outer(j*,j*)) = sum(outer(j,j)) - sum(outer(jnot*,jnot*)) = + = JtJ - Jnot*t Jnot* + + -> l = wt ( 2inv(JtJ) - inv(JtJ)JtJinv(JtJ) + inv(JtJ)Jnot*tJnot* inv(JtJ) ) w + = wt ( inv(JtJ) + inv(JtJ)Jnot*tJnot* inv(JtJ) ) w + + -> substitute -> l = wt ( C + CDC ) w + + where C >= 0 and D >= 0 + + wt C wt >= 0 + + wt CDC wt = (Ctw)t D Ctw >= 0 -> l >= 0 + +So B*B-I >= 0 and dE >= 0: removing a point will never make its own fit better + +- if dE >> 0: the other data does NOT support this measurement being correct +- if dE ~~ 0: the other data supports this measurement being correct + + +Putting all this together I get the expressions for the factors above: + + Dima's self+others: x*t (-B ) x* + Dima's others : x*t (-B - I ) x* + Dima's self (simple): x*t ( I ) x* + Dima's self (interesting): x*t (B*B - I ) x* + Cook's self+others: x*t ( B + B*B) x* + Cook's others : x*t (-B - I ) x* + + + +One can also do a similar analysis to gauge our confidence in a solution. We can +do this by + +1. Solving the optimization problem + +2. Querying the solution in a way we care about to produce a new feature + group of measurements x = f - ref. We can compute J = dx/dp = df/dp. And + we presumably know something about ref: like its probability + distribution for instance. Example: we just calibrated a camera; we want + to know how confident we are about a projection in a particular spot on + the imager. I compute a projection in that spot: q = project(v). If + added to the optimization I'd get x = q - ref where 'ref' would be the + observed pixel coordinate. + +3. If this new feature was added to the optimization, I can compute its + outlierness factor in the same way as before. If we are confident in the + solution in a particular spot, then we have consensus, and it wouldn't take + much for these queries to look like outliers: the expected value of the + outlierness would be high. Conversely, if we aren't well-defined then a wider + set of points would fit the solution, and things wouldn't look very outliery + +Very similar analysis to the above: + +Let p,x,J represent the solution. The new feature we're adding is x* with +jacobian J*. The solution would move by dp to get to the new optimum. + +Original solution is an optimum: Jt x = 0 + +If we add x* and move by dp, we get + + E1 = norm2(x + J dp) + norm2( x* + J* dp ) + +Thus "Dima's self+others factor" is (norm2(x + J dp) + norm2( x* + J* dp )) - +norm2(x) = norm2(Jdp) + norm2( x* + J* dp ) + +We can choose to look at ONLY the effect on the other variables. That would +produce "Dima's others factor" norm2(x + J dp) - norm2(x) = norm2(Jdp) + +This is very similar to Cook's D factor (which Dima apparently reinvented 40 +years after the fact!). This factor looks not at the difference of norm2(x), but +at norm2(difference) instead. So "Cook's others factor" is proportional to +norm2(x - (x+J dp)). This is the "normal" Cook's D factor. We can also compute +"Cook's self+others factor": norm2(concat(x + J dp, x* + J* dp) - concat(x,x*)) += norm2(x + J dp-x) + norm2(x* + J* dp - x*) = norm2(J dp) + norm2(J* dp) + +This is 4 flavors of a very similar computation. In summary (and ignoring scale +factors): + + Dima's self+others: norm2(J dp) + 2 x*t J* dp + norm2(J* dp) + norm2(x*) + Dima's others : norm2(J dp) + Cook's self+others: norm2(J dp) + norm2(J* dp) + Cook's others : norm2(J dp) + +The problem including the new point is also at an optimum: + + E1 = norm2(x + J dp) + norm2( x* + J* dp ) + dE1/dp = 0 -> 0 = Jt x + JtJ dp + J*t x* + J*tJ*dp = + = JtJ dp + J*t x* + J*tJ*dp +-> dp = -inv(JtJ + J*tJ*) J*t x* + +Woodbury identity: + + -inv(JtJ + J*t J*) = + = -inv(JtJ) + inv(JtJ) J*t inv(I + J* inv(JtJ) J*t) J* inv(JtJ) + +Let + A = J* inv(JtJ) J*t (same as before) + B = inv(A + I) (NOT the same as before) + +So + AB = BA = I-B + +Thus + -inv(JtJ + J*t J*) = + = -inv(JtJ) + inv(JtJ) J*t B J* inv(JtJ) + +and + + dp = -inv(JtJ + J*tJ*) J*t x* = + = -inv(JtJ)J*t x* + inv(JtJ) J*t B J* inv(JtJ) J*t x* = + = -inv(JtJ)J*t x* + inv(JtJ) J*t B A x* + = -inv(JtJ)J*t(I - B A) x* + = -inv(JtJ)J*t B x* (same as before, but with a different B!) + +Then + + norm2(J dp) = x*t ( B J* inv() Jt J inv() J*t B ) x* + = x*t ( B J* inv() J*t B ) x* + = x*t ( B A B ) x* + = x*t ( B - B*B ) x* + + 2 x*t J* dp = -2 x*t J* inv(JtJ)J*t B x* = + = -2 x*t A B x* = + = x*t (-2AB) x* = + = x*t (-2I + 2B) x* + + norm2(J* dp) = x*t ( B J* inv() J*tJ* inv() J*t B ) x* = + = x*t ( B A A B ) x* = + = x*t ( I - 2B + B*B ) x* + + norm2(x*) = x*t ( I ) x* + +How do I compute "Dima's self" factor? The "simple" flavor from above looks at +the new measurement only: norm2(x*). The "interesting" flavor, look at what +happens to the measurements' error when they're added to the optimization set. +State moves by dp. x* moves by J* dp. I look at + + dE = norm2(x* + J*dp) - norm2(x*) = + 2 x*t J* dp + norm2(J* dp) = + x*t (-2I + 2B + I - 2B + B*B) x* = + x*t (B*B - I) x* + +I expect that adding a point to the optimization would make it fit better: dE < +0. Let's check. Let's say that there's v,l such that + + (B*B-I)v = l v, norm2(v) = 1 + --> + BBv = (l+1)v + vBBv = l+1 + + Let u = Bv -> + norm2(u) = l+1 + + A = J* inv(JtJ) J*t + B = inv(A + I) -> + + v = (A+I)u + norm2(v) = 1 = norm2(Au) + 2ut A u + norm2(u) -> + -> l = -2ut A u - norm2(Au) + + Let w = J*t u + -> Au = J* inv(JtJ) w + -> ut A u = wt inv(JtJ) w -> + l = -2 wt inv(JtJ) w - norm2(Au) + + Since inv(JtJ) > 0 -> l < 0. As expected + +So B*B-I is negative definite: adding measurements to the optimization set makes +them fit better + +Putting all this together I get the expressions for the factors above: + + Dima's self+others: x*t (B ) x* + Dima's others : x*t (B - B*B) x* + Dima's self (simple): x*t (I ) x* + Dima's self (interesting): x*t (B*B - I) x* + Cook's self+others: x*t (I - B ) x* + Cook's others : x*t (B - B*B) x* + +These expressions are all tested and verified in +mrcal/analyses/outlierness-test.py + + +There're several slightly-different definitions of Cook's D and of a +rule-of-thumb threshold floating around on the internet. Wikipedia says: + + D = norm2(x_io - x_i)^2 / (Nstate * norm2(x_io)/(Nmeasurements - Nstate)) + D_threshold = 1 + +An article https://www.nature.com/articles/nmeth.3812 says + + D = norm2(x_io - x_i)^2 / ((Nstate+1) * norm2(x_io)/(Nmeasurements - Nstate -1)) + D_threshold = 4/Nmeasurements + +I haven't tracked down the reference for this second definition, but it looks +more reasonable, and I use it here. + +Here I use the second definition. That definition expands to + + k = xo^2 / ((Nstate+1) * norm2(x)/(Nmeasurements - Nstate -1)) + B = 1.0/(jt inv(JtJ) j - 1) + f = k * (B + B*B) + +I report normalized outlierness factors so that the threshold is 1. Thus I use + + k = Nmeasurements / (4*((Nstate+1) * norm2(x)/(Nmeasurements - Nstate - 1))) + +*/ + + +static void accum_outlierness_factor(// output + double* factor, + + // inputs + const double* x, + + // A is symmetric. I store the upper triangle row-first + const double* A, + + // if outliers are grouped into features, the + // feature size is set here + int featureSize, + double k) +{ + // This implements Dima's self+outliers factor. + // + // from the derivation in a big comment above I have + // + // f = k (x*t (-B) x* ) + // + // where B = inv(A - I) and + // A = J* inv(JtJ) J*t + + // I only implemented featureSize == 1 and 2 so far. + if(featureSize <= 1) + { + featureSize = 1; + + double denom = 1.0 - *A; + if( fabs(denom) < 1e-8 ) + { + *factor = DBL_MAX; // definitely an outlier + return; + } + else + *factor = x[0]*x[0] / denom; + } + else if(featureSize == 2) + { + double det = (1.0-A[0])*(1.0-A[2]) - A[1]*A[1]; + if( fabs(det) < 1e-8 ) + { + *factor = DBL_MAX; // definitely an outlier + return; + } + else + { + double B00_det = A[2] - 1.0; + double B11_det = A[0] - 1.0; + double B01_det = -A[1]; + + // inner(x, Bx) + double xBx = + (x[0]*x[0]*B00_det + + x[0]*x[1]*B01_det * 2.0 + + x[1]*x[1]*B11_det) / det; + + // norm2(Bx) + double v1 = x[0]*B00_det + x[1]*B01_det; + double v2 = x[0]*B01_det + x[1]*B11_det; + double xBBx = (v1*v1 + v2*v2) / (det*det); + + // // mine self+others + // *factor = -xBx; + + // // mine others / cook others + // *factor = -(x[0]*x[0] + x[1]*x[1]) - xBx; + + // cook's self+others + *factor = xBx + xBBx; + + } + } + else + { + SAY("featureSize > 2 not implemented yet. Got featureSize=%d", featureSize); + ASSERT(0); + } + + +// #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.; + + + *factor *= k; +} + +static void getOutliernessScale(// out + double* scale, // if *scale > 0, I just keep what I have + + // in + int Nmeasurements, int Nstate, + int NoutlierFeatures, int featureSize, + double norm2_x) +{ + // if *scale > 0, I just keep what I have + if(*scale > 0.0) + return; + + int Nmeasurements_outliers = NoutlierFeatures*featureSize; + int Nmeasurements_nonoutliers = Nmeasurements - Nmeasurements_outliers; + + *scale = + (double)(Nmeasurements_nonoutliers) / + (4.*((double)(Nstate+1) * norm2_x/(double)(Nmeasurements_nonoutliers - Nstate - 1))); +} + +static bool getOutliernessFactors_dense( // output + double* factors, // Nfeatures factors + + // output, input + double* scale, // if <0 then I recompute + + // inputs + // if outliers are grouped into features, + // the feature size is set here + int featureSize, + int Nfeatures, + int NoutlierFeatures, + const dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx ) +{ + // cholmod_spsolve() and cholmod_solve()) work in chunks of 4, so I do this in + // chunks of 4 too. I pass it rows of J, 4 at a time. Note that if I have + // measurement features, I don't want these to cross chunk boundaries, so I set + // up chunk_size=lcm(N,4) + int chunk_size = 4; + if(featureSize <= 1) + featureSize = 1; + if(featureSize > 1) + { + // haven't implemented anything else yet. Don't have lcm() readily available + ASSERT(featureSize == 2); + // chunk_size = lcm(chunk_size,featureSize) + } + + int Nstate = ctx->Nstate; + int Nmeasurements = ctx->Nmeasurements; + bool result = false; + + double* invJtJ_Jt = static_cast(malloc(Nstate*chunk_size*sizeof(double))); + if(invJtJ_Jt == NULL) + { + SAY("Couldn't allocate invJtJ_Jt!"); + free(invJtJ_Jt); + return result; + } + + getOutliernessScale(scale, + Nmeasurements, Nstate, NoutlierFeatures, featureSize, point->norm2_x); + + int i_measurement_valid_chunk_start = -1; + int i_measurement_valid_chunk_last = -1; + int i_measurement = 0; + for( int i_feature=0; i_feature i_measurement_valid_chunk_last ) + { + bool pinvresult = pseudoinverse_J_dense(invJtJ_Jt, point, ctx, + i_measurement, chunk_size); + if(!pinvresult) + { + SAY("Couldn't compute pinv!"); + free(invJtJ_Jt); + return result; + } + i_measurement_valid_chunk_start = i_measurement; + i_measurement_valid_chunk_last = i_measurement+chunk_size-1; + } + + // from the derivation in a big comment in above I have + // + // f = scale (xot (...) xo ) + // + // where A = Jo inv(JtJ) Jot + // + // A is symmetric. I store the upper triangle + std::vector A(featureSize*(featureSize+1)/2); + int iA=0; + for(int i=0; iJ_dense[Nstate* i_measurement+j + k]; + } + accum_outlierness_factor(&factors[i_feature], + &point->x[i_measurement], + A.data(), featureSize, *scale); + } + + result = true; + free(invJtJ_Jt); + return result; +} + + +static bool getOutliernessFactors_sparse( // output + double* factors, // Nfeatures factors + + // output, input + double* scale, // if <0 then I recompute + + // inputs + // if outliers are grouped into features, + // the feature size is set here + int featureSize, + int Nfeatures, + int NoutlierFeatures, + const dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx ) +{ + // cholmod_spsolve() and cholmod_solve()) work in chunks of 4, so I do this in + // chunks of 4 too. I pass it rows of J, 4 at a time. Note that if I have + // measurement features, I don't want these to cross chunk boundaries, so I set + // up chunk_size=lcm(N,4) + int chunk_size = 4; + if(featureSize <= 1) + featureSize = 1; + if(featureSize > 1) + { + // haven't implemented anything else yet. Don't have lcm() readily available + ASSERT(featureSize == 2); + // chunk_size = lcm(chunk_size,featureSize) + } + + int Nstate = ctx->Nstate; + int Nmeasurements = ctx->Nmeasurements; + bool result = false; + + cholmod_dense* invJtJ_Jt = NULL; + cholmod_dense* Jt_chunk = + cholmod_allocate_dense( Nstate, + chunk_size, + Nstate, + CHOLMOD_REAL, + &ctx->common ); + if(!Jt_chunk) + { + SAY("Couldn't allocate Jt_chunk!"); + if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + return result; + } + + getOutliernessScale(scale, + Nmeasurements, Nstate, NoutlierFeatures, featureSize, point->norm2_x); + + int i_measurement_valid_chunk_start = -1; + int i_measurement_valid_chunk_last = -1; + int i_measurement = 0; + for( int i_feature=0; i_feature i_measurement_valid_chunk_last ) + { + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + invJtJ_Jt = pseudoinverse_J_sparse(point, ctx, + i_measurement, chunk_size, + Jt_chunk); + if(invJtJ_Jt == NULL) + { + SAY("Couldn't compute pinv!"); + if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + return result; + } + + i_measurement_valid_chunk_start = i_measurement; + i_measurement_valid_chunk_last = i_measurement+chunk_size-1; + } + + // from the derivation in a big comment in above I have + // + // f = scale (xot (...) xo ) + // + // where A = Jo inv(JtJ) Jot + // + // A is symmetric. I store the upper triangle + std::vector A(featureSize*(featureSize+1)/2); + int iA=0; + for(int i=0; iJt, i_measurement+j); + l < P(point->Jt, i_measurement+j+1); + l++) + { + int k = I(point->Jt, l); + A[iA] += + ((double*)invJtJ_Jt->x)[Nstate*(i_measurement+i-i_measurement_valid_chunk_start) + k] * + X(point->Jt, l); + } + } + accum_outlierness_factor(&factors[i_feature], + &point->x[i_measurement], + A.data(), featureSize, *scale); + } + + result = true; + if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common); + if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common); + return result; +} + +bool dogleg_getOutliernessFactors( // output + double* factors, // Nfeatures factors + + // output, input + double* scale, // if <0 then I recompute + + // inputs + // if outliers are grouped into features, the + // feature size is set here + int featureSize, + int Nfeatures, + int NoutlierFeatures, // how many outliers we already have + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx ) +{ + if(featureSize <= 1) + featureSize = 1; + + dogleg_computeJtJfactorization( point, ctx ); + bool result; + if(ctx->is_sparse) + result = getOutliernessFactors_sparse(factors, scale, featureSize, Nfeatures, NoutlierFeatures, point, ctx); + else + result = getOutliernessFactors_dense(factors, scale, featureSize, Nfeatures, NoutlierFeatures, point, ctx); + +#if 0 + if( result ) + { + int Nstate = ctx->Nstate; + int Nmeasurements = ctx->Nmeasurements; + + + + static FILE* fp = NULL; + if(fp == NULL) + fp = fopen("/tmp/check-outlierness.py", "w"); + static int count = -1; + count++; + if(count > 5) + goto done; + + fprintf(fp, "# WARNING: all J here are unscaled with SCALE_....\n"); + + if(count == 0) + { + fprintf(fp, + "import numpy as np\n" + "import numpysane as nps\n" + "np.set_printoptions(linewidth=100000)\n" + "\n"); + } + + fprintf(fp, "x%d = np.array((", count); + for(int j=0;jx[j]); + fprintf(fp,"))\n"); + + if( ctx->is_sparse ) + { + fprintf(fp, "J%d = np.zeros((%d,%d))\n", count, Nmeasurements, Nstate); + for(int imeas=0;imeasJt, imeas); + j < (int)P(point->Jt, imeas+1); + j++) + { + int irow = I(point->Jt, j); + fprintf(fp, "J%d[%d,%d] = %.20g\n", count, + imeas, irow, X(point->Jt, j)); + } + } + } + else + { + fprintf(fp, "J%d = np.array((\n", count); + for(int j=0;jJ_dense[j*Nstate+i]); + fprintf(fp, "),\n"); + } + fprintf(fp,"))\n"); + } + + fprintf(fp, "Nmeasurements = %d\n", Nmeasurements); + fprintf(fp, "Nstate = %d\n", Nstate); + fprintf(fp, "Nfeatures = %d\n", Nfeatures); + fprintf(fp, "featureSize = %d\n", featureSize); + fprintf(fp, "NoutlierFeatures = %d\n", NoutlierFeatures); + + fprintf(fp, "scale_got = %.20f\n", *scale); + + fprintf(fp, "factors_got = np.array(("); + for(int j=0;j + B = inv(I + A) -> + tr(B) = (2+a00+a11) / ((1+a00)*(1+a11) - a01^2) + */ + + + // This is Jt because cholmod thinks in terms of col-first instead of + // row-first + std::vector Jt_p(featureSize+1); + std::vector Jt_i(NstateActive*featureSize); + for(int i=0; i<=featureSize; i++) + { + Jt_p[i] = i*NstateActive; + if(i==featureSize) break; + for(int j=0; j(ctx->Nstate), + .ncol = static_cast(featureSize), + .nzmax = static_cast(NstateActive*featureSize), + .p = Jt_p.data(), + .i = Jt_i.data(), + .x = const_cast(JqueryFeature), + .stype = 0, // NOT symmetric + .itype = CHOLMOD_INT, + .xtype = CHOLMOD_REAL, + .dtype = CHOLMOD_DOUBLE, + .sorted = 1, + .packed = 1}; + + // Really shouldn't need to do this every time. In fact I probably don't need + // to do it at all, since this will have been done by the solver during the + // last step + dogleg_computeJtJfactorization( point, ctx ); + cholmod_sparse* invJtJ_Jp = + cholmod_spsolve(CHOLMOD_A, + ctx->factorization, + &Jt_query_sparse, + &ctx->common); + + // Now I need trace(matmult(Jquery, invJtJ_Jp)) + + // haven't implemented anything else yet + ASSERT(featureSize == 2); + double A[4] = {}; // gah. only elements 0,1,3 will be stored. + + for(int i=0; i= istateActive) + { + if(row >= istateActive+NstateActive) + break; + + int ic0 = i*featureSize; + for(int k=i; kcommon); + + double invB00 = A[0]+1.0; + double invB01 = A[1]; + double invB11 = A[3]+1.0; + + double det_invB_recip = 1.0/(invB00*invB11 - invB01*invB01); + double B00 = invB11 * det_invB_recip; + double B11 = invB00 * det_invB_recip; + + + double B01 = -invB01 * det_invB_recip; + double traceB = B00 + B11; + + double traceBB = B00*B00 + 2.0*B01*B01 + B11*B11; + + +#if 0 + static int count = -1; + count++; + if(count <= 5) + { + int Nstate = ctx->Nstate; + int Nmeasurements = ctx->Nmeasurements; + + + + static FILE* fp = NULL; + if(fp == NULL) + fp = fopen("/tmp/check-query-outlierness.py", "w"); + + fprintf(fp, "# WARNING: all J here are unscaled with SCALE_....\n"); + + if(count == 0) + { + fprintf(fp, + "import numpy as np\n" + "import numpysane as nps\n" + "np.set_printoptions(linewidth=100000)\n" + "\n"); + } + + fprintf(fp, "x%d = np.array((", count); + for(int j=0;jx[j]); + fprintf(fp,"))\n"); + + if( ctx->is_sparse ) + { + fprintf(fp, "J%d = np.zeros((%d,%d))\n", count, Nmeasurements, Nstate); + for(int imeas=0;imeasJt, imeas); + j < (int)P(point->Jt, imeas+1); + j++) + { + int irow = I(point->Jt, j); + fprintf(fp, "J%d[%d,%d] = %g\n", count, + imeas, irow, X(point->Jt, j)); + } + } + } + else + { + fprintf(fp, "J%d = np.array((\n", count); + for(int j=0;jJ_dense[j*Nstate+i]); + fprintf(fp, "),\n"); + } + fprintf(fp,"))\n"); + } + + fprintf(fp, "Nmeasurements = %d\n", Nmeasurements); + fprintf(fp, "Nstate = %d\n", Nstate); + fprintf(fp, "featureSize = %d\n", featureSize); + fprintf(fp, "traceB_got = %.20g\n", traceB); + + fprintf(fp, "Jq = np.zeros((featureSize, Nstate), dtype=float)\n"); + for(int i=0; iNmeasurements; + int Nstate = ctx->Nstate; + + double scale = -1.0; + getOutliernessScale(&scale, + Nmeasurements, Nstate, NoutlierFeatures, featureSize, point->norm2_x); + + // // Dima's self+others + // return scale * traceB; + + // Cook's self+others + return scale * (2.0 - traceB); + + // // Dima's others/Cook's others + // // This one is non-monotonic in outlierness-test + // return scale * (traceB - traceBB); + +} + + +#define OUTLIER_CONFIDENCE_DROP_THRESHOLD 0.05 +bool dogleg_markOutliers(// output, input + struct dogleg_outliers_t* markedOutliers, + double* scale, // if <0 then I recompute + + // output, input + int* Noutliers, + + // input + double (getConfidence)(int i_feature_exclude), + + // if outliers are grouped into features, the feature + // size is set here + int featureSize, + int Nfeatures, + + dogleg_operatingPoint_t* point, + dogleg_solverContext_t* ctx) +{ + if(featureSize <= 1) + featureSize = 1; + + bool markedAny = false; + + double* factors = static_cast(malloc(Nfeatures * sizeof(double))); + if(factors == NULL) + { + SAY("Error allocating factors"); + free(factors); + return markedAny; + } + + if(!dogleg_getOutliernessFactors(factors, scale, + featureSize, Nfeatures, + *Noutliers, + point, ctx)) + { + free(factors); + return markedAny; + } + + // I have my list of POTENTIAL outliers (any that have factor > 1.0). I + // check to see how much confidence I would lose if I were to throw out any + // of these measurements, and accept the outlier ONLY if the confidence loss + // is acceptable + double confidence0 = getConfidence(-1); + if( confidence0 < 0.0 ) + { + free(factors); + return markedAny; + } + + SAY_IF_VERBOSE("Initial confidence: %g", confidence0); + + *Noutliers = 0; + for(int i=0; i(malloc(Nfeatures * sizeof(double))); + if(factors == NULL) + { + SAY("Error allocating factors"); + free(factors); + } + + dogleg_getOutliernessFactors(factors, scale, + featureSize, Nfeatures, Noutliers, point, ctx); + + SAY("## Outlier statistics"); + SAY("# i_feature outlier_factor confidence_drop_relative_if_removed"); + + double confidence_full = getConfidence(-1); + + for(int i=0; i vout +static inline void add_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout) +{ + for(int i=0; i vout +static inline void sub_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout) +{ + for(int i=0; i vout +static inline void add_vec_vout_scaled(int n, const double* restrict a, const double* restrict b, double* restrict vout, double scale) +{ + for(int i=0; i vout +static inline void sub_vec_vout_scaled(int n, const double* restrict a, const double* restrict b, double* restrict vout, double scale) +{ + for(int i=0; i a +static inline void add_vec(int n, double* restrict a, const double* restrict b) +{ + for(int i=0; i a +static inline void sub_vec(int n, double* restrict a, const double* restrict b) +{ + for(int i=0; i a +static inline void add_vec_scaled(int n, double* restrict a, const double* restrict b, double scale) +{ + for(int i=0; i a +static inline void sub_vec_scaled(int n, double* restrict a, const double* restrict b, double scale) +{ + for(int i=0; i + vaccum +static inline void add_vec_vaccum(int n, const double* restrict a, const double* restrict b, double* restrict vaccum) +{ + for(int i=0; i + vaccum +static inline void sub_vec_vaccum(int n, const double* restrict a, const double* restrict b, double* restrict vaccum) +{ + for(int i=0; i +#include +#include "strides.h" + +template struct vec_withgrad_t; + + +template +struct val_withgrad_t +{ + double x; + double j[NGRAD == 0 ? 1 : NGRAD]; + + + 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; + y.x += b.x; + for(int i=0; i operator+( double b ) const + { + val_withgrad_t y = *this; + y.x += b; + return y; + } + + void operator+=( const val_withgrad_t& b ) + { + *this = (*this) + b; + } + + val_withgrad_t operator-( const val_withgrad_t& b ) const + { + val_withgrad_t y = *this; + y.x -= b.x; + for(int i=0; i operator-( double b ) const + { + val_withgrad_t y = *this; + y.x -= b; + return y; + } + + void operator-=( const val_withgrad_t& b ) + { + *this = (*this) - b; + } + + val_withgrad_t operator-() const + { + return (*this) * (-1); + } + + val_withgrad_t operator*( const val_withgrad_t& b ) const + { + val_withgrad_t y; + y.x = x * b.x; + for(int i=0; i operator*( double b ) const + { + val_withgrad_t y; + y.x = x * b; + for(int i=0; i& b ) + { + *this = (*this) * b; + } + + void operator*=( const double b ) + { + *this = (*this) * b; + } + + val_withgrad_t operator/( const val_withgrad_t& b ) const + { + val_withgrad_t y; + y.x = x / b.x; + for(int i=0; i operator/( double b ) const + { + return (*this) * (1./b); + } + + void operator/=( const val_withgrad_t& b ) + { + *this = (*this) / b; + } + + void operator/=( const double b ) + { + *this = (*this) / b; + } + + val_withgrad_t sqrt(void) const + { + val_withgrad_t y; + y.x = ::sqrt(x); + for(int i=0; i square(void) const + { + val_withgrad_t s; + s.x = x*x; + for(int i=0; i sin(void) const + { + const double s = ::sin(x); + const double c = ::cos(x); + val_withgrad_t y; + y.x = s; + for(int i=0; i cos(void) const + { + const double s = ::sin(x); + const double c = ::cos(x); + val_withgrad_t y; + y.x = c; + for(int i=0; i sincos(void) const + { + const double s = ::sin(x); + const double c = ::cos(x); + vec_withgrad_t sc; + sc.v[0].x = s; + sc.v[1].x = c; + for(int i=0; i tan(void) const + { + const double s = ::sin(x); + const double c = ::cos(x); + val_withgrad_t y; + y.x = s/c; + for(int i=0; i atan2(val_withgrad_t& x) const + { + val_withgrad_t th; + const val_withgrad_t& y = *this; + + th.x = ::atan2(y.x, x.x); + // dth/dv = d/dv atan2(y,x) + // = d/dv atan(y/x) + // = 1 / (1 + y^2/x^2) d/dv (y/x) + // = x^2 / (x^2 + y^2) / x^2 * (dy/dv x - y dx/dv) + // = 1 / (x^2 + y^2) * (dy/dv x - y dx/dv) + double norm2 = y.x*y.x + x.x*x.x; + for(int i=0; i 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; + th.x = ::acos(x); + double dacos_dx = -1. / ::sqrt( 1. - x*x ); + for(int i=0; i sinx_over_x(// To avoid recomputing it + const val_withgrad_t& sinx) const + { + // For small x I need special-case logic. In the limit as x->0 I have + // sin(x)/x -> 1. But I'm propagating gradients, so I need to capture + // that. I have + // + // d(sin(x)/x)/dx = + // (x cos(x) - sin(x))/x^2 + // + // As x -> 0 this is + // + // (cos(x) - x sin(x) - cos(x)) / (2x) = + // (- x sin(x)) / (2x) = + // -sin(x) / 2 = + // 0 + // + // So for small x the gradient is 0 + if(fabs(x) < 1e-5) + return val_withgrad_t(1.0); + + return sinx / (*this); + } +}; + + +template +struct vec_withgrad_t +{ + val_withgrad_t v[NVEC == 0 ? 1 : NVEC]; + + vec_withgrad_t() {} + + + void init_vars(const double* x_in, int ivar0, int Nvars, int i_gradvec0 = -1, + int stride = sizeof(double)) + { + // Initializes vector entries ivar0..ivar0+Nvars-1 inclusive using the + // data in x_in[]. x_in[0] corresponds to vector entry ivar0. If + // i_gradvec0 >= 0 then vector ivar0 corresponds to gradient index + // i_gradvec0, with all subsequent entries being filled-in + // consecutively. It's very possible that NGRAD > Nvars. Initially the + // subset of the gradient array corresponding to variables + // i_gradvec0..i_gradvec0+Nvars-1 is an identity, with the rest being 0 + memset((char*)&v[ivar0], 0, Nvars*sizeof(v[0])); + for(int i=ivar0; i= 0) + v[i].j[i_gradvec0+i-ivar0] = 1.0; + } + } + + + vec_withgrad_t(const double* x_in, int i_gradvec0 = -1, + int stride = sizeof(double)) + { + init_vars(x_in, 0, NVEC, i_gradvec0, stride); + } + + + val_withgrad_t& operator[](int i) + { + return v[i]; + } + + + const val_withgrad_t& operator[](int i) const + { + return v[i]; + } + + + void operator+=( const vec_withgrad_t& x ) + { + (*this) = (*this) + x; + } + + vec_withgrad_t operator+( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) + x; + } + + vec_withgrad_t operator+( const val_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i operator+( double x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) - x; + } + + vec_withgrad_t operator-( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) - x; + } + + vec_withgrad_t operator-( const val_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i operator-( double x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) * x; + } + + vec_withgrad_t operator*( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) * x; + } + + vec_withgrad_t operator*( const val_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i operator*( double x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) / x; + } + + vec_withgrad_t operator/( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) / x; + } + + vec_withgrad_t operator/( const val_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i operator/( double x ) const + { + vec_withgrad_t p; + for(int i=0; i dot( const vec_withgrad_t& x) const + { + val_withgrad_t d; // initializes to 0 + for(int i=0; i e = x.v[i]*v[i]; + d += e; + } + return d; + } + + + vec_withgrad_t cross( const vec_withgrad_t& x) const + { + vec_withgrad_t c; + c[0] = v[1]*x.v[2] - v[2]*x.v[1]; + c[1] = v[2]*x.v[0] - v[0]*x.v[2]; + c[2] = v[0]*x.v[1] - v[1]*x.v[0]; + return c; + } + + + val_withgrad_t norm2(void) const + { + return dot(*this); + } + + + val_withgrad_t mag(void) const + { + val_withgrad_t l2 = norm2(); + return l2.sqrt(); + } + + + void extract_value(double* out, + int stride = sizeof(double), + int ivar0 = 0, int Nvars = NVEC) const + { + for(int i=ivar0; i +static +vec_withgrad_t +cross( const vec_withgrad_t& a, + const vec_withgrad_t& b ) +{ + vec_withgrad_t c; + c.v[0] = a.v[1]*b.v[2] - a.v[2]*b.v[1]; + c.v[1] = a.v[2]*b.v[0] - a.v[0]*b.v[2]; + c.v[2] = a.v[0]*b.v[1] - a.v[1]*b.v[0]; + return c; +} + +template +static +val_withgrad_t +cross_norm2( const vec_withgrad_t& a, + const vec_withgrad_t& b ) +{ + vec_withgrad_t c = cross(a,b); + return c.norm2(); +} + +template +static +val_withgrad_t +cross_mag( const vec_withgrad_t& a, + const vec_withgrad_t& b ) +{ + vec_withgrad_t c = cross(a,b); + return c.mag(); +} diff --git a/wpical/src/main/native/thirdparty/mrcal/include/basic-geometry.h b/wpical/src/main/native/thirdparty/mrcal/include/basic-geometry.h new file mode 100644 index 00000000000..13bd2e919fe --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/basic-geometry.h @@ -0,0 +1,130 @@ +// 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 + +#pragma once + +// A 2D point or vector +// +// The individual elements can be accessed via .x and .y OR the vector can be +// accessed as an .xy[] array: +// +// mrcal_point2_t p = f(); +// +// Now p.x and p.xy[0] refer to the same value. +typedef union +{ + struct + { + double x,y; + }; + + double xy[2]; +} mrcal_point2_t; + +// A 3D point or vector +// +// The individual elements can be accessed via .x and .y and .z OR the vector +// can be accessed as an .xyz[] array: +// +// mrcal_point3_t p = f(); +// +// Now p.x and p.xy[0] refer to the same value. +typedef union +{ + struct + { + double x,y,z; + }; + double xyz[3]; +} mrcal_point3_t; + +// Unconstrained 6DOF pose containing a Rodrigues rotation and a translation +typedef struct +{ + mrcal_point3_t r,t; +} mrcal_pose_t; + + +//////////// Easy convenience stuff +////// point2 + +static double mrcal_point2_inner(const mrcal_point2_t a, const mrcal_point2_t b) +{ + return + a.x*b.x + + a.y*b.y; +} + +static double mrcal_point2_norm2(const mrcal_point2_t a) +{ + return mrcal_point2_inner(a,a); +} +#define mrcal_point2_mag(a) sqrt(norm2(a)) // macro to not require #include + + +static mrcal_point2_t mrcal_point2_add(const mrcal_point2_t a, const mrcal_point2_t b) +{ + return (mrcal_point2_t){ .x = a.x + b.x, + .y = a.y + b.y }; +} + +static mrcal_point2_t mrcal_point2_sub(const mrcal_point2_t a, const mrcal_point2_t b) +{ + return (mrcal_point2_t){ .x = a.x - b.x, + .y = a.y - b.y }; +} + +static mrcal_point2_t mrcal_point2_scale(const mrcal_point2_t a, const double s) +{ + return (mrcal_point2_t){ .x = a.x * s, + .y = a.y * s }; +} +////// point3 + +static double mrcal_point3_inner(const mrcal_point3_t a, const mrcal_point3_t b) +{ + return + a.x*b.x + + a.y*b.y + + a.z*b.z; +} + +static double mrcal_point3_norm2(const mrcal_point3_t a) +{ + return mrcal_point3_inner(a,a); +} +#define mrcal_point3_mag(a) sqrt(mrcal_point3_norm2(a)) // macro to not require #include + + +static mrcal_point3_t mrcal_point3_add(const mrcal_point3_t a, const mrcal_point3_t b) +{ + return (mrcal_point3_t){ .x = a.x + b.x, + .y = a.y + b.y, + .z = a.z + b.z }; +} + +static mrcal_point3_t mrcal_point3_sub(const mrcal_point3_t a, const mrcal_point3_t b) +{ + return (mrcal_point3_t){ .x = a.x - b.x, + .y = a.y - b.y, + .z = a.z - b.z }; +} + +static mrcal_point3_t mrcal_point3_scale(const mrcal_point3_t a, const double s) +{ + return (mrcal_point3_t){ .x = a.x * s, + .y = a.y * s, + .z = a.z * s }; +} + +static mrcal_point3_t mrcal_point3_cross(const mrcal_point3_t a, const mrcal_point3_t b) +{ + return (mrcal_point3_t){ .x = a.y*b.z - a.z*b.y, + .y = a.z*b.x - a.x*b.z, + .z = a.x*b.y - a.y*b.x }; +} diff --git a/wpical/src/main/native/thirdparty/mrcal/include/cahvore.h b/wpical/src/main/native/thirdparty/mrcal/include/cahvore.h new file mode 100644 index 00000000000..4b6d3ddaddd --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/cahvore.h @@ -0,0 +1,23 @@ +// 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 + +#pragma once + +#include +#include "basic-geometry.h" + +bool project_cahvore_internals( // outputs + mrcal_point3_t* __restrict pdistorted, + double* __restrict dpdistorted_dintrinsics_nocore, + double* __restrict dpdistorted_dp, + + // inputs + const mrcal_point3_t* __restrict p, + const double* __restrict intrinsics_nocore, + const double cahvore_linearity); + diff --git a/wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h b/wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h new file mode 100644 index 00000000000..592e30694f9 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h @@ -0,0 +1,1988 @@ +#pragma once + +// Extra functions I'm using in mrcal. I'm going to replace this whole library +// eventually, to make things nicer. These new functions will be a part of the +// replacement, and I'm not going to be thorough and I'm not going to add tests +// until I do that. + +#define restrict __restrict + +// Upper triangle is stored, in the usual row-major order. + +static +int index_sym33(int i, int j) +{ + // In the top-right triangle I have i<=j, and + // index = N*i+j - sum(i, i=0..i) + // = N*i+j - (i+1)*i/2 + // = (N*2 - i - 1)*i/2 + j + const int N=3; + if(i<=j) return (N*2-i-1)*i/2 + j; + else return (N*2-j-1)*j/2 + i; +} + +static +int index_sym33_assume_upper(int i, int j) +{ + const int N=3; + return (N*2-i-1)*i/2 + j; +} + +// Upper triangle is stored, in the usual row-major order. + +static +int index_sym66(int i, int j) +{ + // In the top-right triangle I have i<=j, and + // index = N*i+j - sum(i, i=0..i) + // = N*i+j - (i+1)*i/2 + // = (N*2 - i - 1)*i/2 + j + const int N=6; + if(i<=j) return (N*2-i-1)*i/2 + j; + else return (N*2-j-1)*j/2 + i; +} + +static +int index_sym66_assume_upper(int i, int j) +{ + const int N=6; + return (N*2-i-1)*i/2 + j; +} + +static +void mul_gen33_gen33insym66(// output + double* restrict P, int P_strideelems0, int P_strideelems1, + // input + const double* A, int A_strideelems0, int A_strideelems1, + const double* Bsym66, int B_i0, int B_j0, + const double scale) +{ + for(int iout=0; iout<3; iout++) + for(int jout=0; jout<3; jout++) + { + P[iout*P_strideelems0 + jout*P_strideelems1] = 0; + for(int k=0; k<3; k++) + { + P[iout*P_strideelems0 + jout*P_strideelems1] += + A[iout*A_strideelems0 + k*A_strideelems1] * + Bsym66[index_sym66(k+B_i0, jout+B_j0)]; + } + P[iout*P_strideelems0 + jout*P_strideelems1] *= scale; + } +} +// Assumes the output is symmetric, and only computes the upper triangle + +static +void mul_gen33_gen33_into33insym66_accum(// output + double* restrict Psym66, int P_i0, int P_j0, + // input + const double* A, int A_strideelems0, int A_strideelems1, + const double* B, int B_strideelems0, int B_strideelems1, + const double scale) +{ + for(int iout=0; iout<3; iout++) + for(int jout=0; jout<3; jout++) + { + if(jout + P_j0 < iout + P_i0) + { + // Wrong triangle. Set it up to look at the right triangle + // during the next loop interation + jout = iout + P_i0 - P_j0 - 1; + continue; + } + + for(int k=0; k<3; k++) + { + Psym66[index_sym66_assume_upper(iout+P_i0, jout+P_j0)] += + A[iout*A_strideelems0 + k *A_strideelems1] * + B[k *B_strideelems0 + jout*B_strideelems1] * scale; + } + } +} + +static +void set_gen33_from_gen33insym66(// output + double* restrict P, int P_strideelems0, int P_strideelems1, + // input + const double* Msym66, int M_i0, int M_j0, + const double scale) +{ + for(int iout=0; iout<3; iout++) + for(int jout=0; jout<3; jout++) + P[iout*P_strideelems0 + jout*P_strideelems1] = + Msym66[index_sym66(iout+M_i0, jout+M_j0)] * scale; +} +// Assumes the output is symmetric, and only computes the upper triangle + +static +void set_33insym66_from_gen33_accum(// output + double* restrict Psym66, int P_i0, int P_j0, + // input + const double* M, int M_strideelems0, int M_strideelems1, + const double scale) +{ + for(int iout=0; iout<3; iout++) + for(int jout=0; jout<3; jout++) + { + if(jout + P_j0 < iout + P_i0) + { + // Wrong triangle. Set it up to look at the right triangle + // during the next loop interation + jout = iout + P_i0 - P_j0 - 1; + continue; + } + + Psym66[index_sym66_assume_upper(iout+P_i0, jout+P_j0)] += + M[iout*M_strideelems0 + jout*M_strideelems1] * scale; + } +} + +// This is completely unreasonable. I'm almost certainly going to replace it + +static +double cofactors_sym6(const double* restrict m, double* restrict c) +{ + /* +Just like in libminimath; adding 6x6 version. I use the maxima result verbatim, except: + +- mNNN -> m[NNN] +- XXX^2 -> XXX*XXX + +maxima session: + +(%i1) display2d : false; + +(%i2) sym6 : matrix([m0,m1, m2, m3, m4, m5 ], + [m1,m6, m7, m8, m9, m10], + [m2,m7, m11, m12,m13, m14], + [m3,m8, m12, m15,m16, m17], + [m4,m9, m13, m16,m18, m19], + [m5,m10,m14, m17,m19, m20]); + +(%i10) num( ev(invert(sym6),detout) ); + +(%o10) matrix([(-m7*((-m12*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m14*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7)) + +m8*((-m11*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m7) + -m9*((-m11*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m12*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m7) + +m10*((-m11*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m12*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m13*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m7) + +(m11*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + -m12*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + +m13*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + -m14*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))) + *m6, + m2*((-m12*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m14*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7) + -m3*((-m11*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m7) + +m4*((-m11*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m12*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m7) + -m5*((-m11*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m12*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m13*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m7) + -m1*(m11*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + -m12*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + +m13*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + -m14*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + (-m2*((-m8*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m9*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m10*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m6)) + +m3*((-m7*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m9*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m10*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m6) + -m4*((-m7*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m8*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m10*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m6) + +m5*((-m7*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m8*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m9*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m6) + +m1*((m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m9 + -(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m8 + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7 + -m10*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + m2*((-m8*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7)) + +m9*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7) + +(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)) + *m6) + -m3*((-m7*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7)) + +m9*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -m10*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + +(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18)) + *m6) + +m4*((-m7*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7)) + +m8*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -m10*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + +(m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16)) + *m6) + -m5*((-m7*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7)) + +m8*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + -m9*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + +(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16)) + *m6) + -m1*((m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16)) + *m9 + -(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18)) + *m8 + +(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)) + *m7 + -m10*(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16))), + (-m2*(((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7) + *m9 + -m8*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + +(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17)) + *m6)) + +m3*(((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + *m9 + -m7*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + +(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16)) + *m6) + +m5*((-((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + *m9) + -m7*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + +m8*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + +(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15)) + *m6) + +m1*((m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15)) + *m9 + -(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16)) + *m8 + +(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17)) + *m7 + -m10*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15))) + -m4*((-m7*((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7)) + +m8*((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + -m10*((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + +(m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15)) + *m6), + m2*((-m8*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7)) + +m9*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7) + -m10*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7) + +(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)) + *m6) + -m3*((-m7*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7)) + +m9*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -m10*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + +(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)) + *m6) + +m4*((-m7*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7)) + +m8*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -m10*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + +(m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)) + *m6) + -m5*((-m7*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7)) + +m8*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + -m9*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + +(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15)) + *m6) + -m1*((m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)) + *m9 + -(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)) + *m8 + +(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)) + *m7 + -m10*(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15)))], + [((-m11*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3)) + +m12*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m2*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))) + *m9 + -((-m11*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m2*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))) + *m8 + +((-m12*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + -m14*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m2*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))) + *m7 + -m10*((-m11*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3)) + +m12*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + -m13*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m2) + -m1*(m11*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + -m12*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + +m13*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + -m14*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + (-m2*((-m12*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + -m14*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m2*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)))) + +m3*((-m11*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m2*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))) + -m4*((-m11*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3)) + +m12*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m2*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))) + +m5*((-m11*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3)) + +m12*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + -m13*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m2) + +m0*(m11*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + -m12*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + +m13*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + -m14*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + m2*(((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m9 + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m8 + -m10*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m1*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))) + -m3*(((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m9 + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7 + -m10*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m1*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))) + -m5*((-((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m9) + +((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m8 + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7 + +m1*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))) + -m0*((m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m9 + -(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m8 + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7 + -m10*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))) + +m4*(((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m8 + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7 + -m10*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m1*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))), + (-m2*(((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m9 + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m8 + -m10*((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + +m1*(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)))) + +m3*(((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m9 + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m7 + -m10*((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + +m1*(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18))) + +m5*((-((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + *m9) + +((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + *m8 + -((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + *m7 + +m1*(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16))) + +m0*((m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16)) + *m9 + -(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18)) + *m8 + +(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)) + *m7 + -m10*(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16))) + -m4*(((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m8 + -((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m7 + -m10*((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + +m1*(m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16))), + m2*(((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m9 + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m8 + -m10*((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + +m1*(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17))) + -m3*(((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m9 + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m7 + -m10*((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + +m1*(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16))) + -m5*((-((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + *m9) + +((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + *m8 + -((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + *m7 + +m1*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15))) + -m0*((m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15)) + *m9 + -(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16)) + *m8 + +(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17)) + *m7 + -m10*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15))) + +m4*(((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m8 + -((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m7 + -m10*((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + +m1*(m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15))), + (-m2*(((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m9 + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m8 + -m10*((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + +m1*(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)))) + +m3*(((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m9 + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m7 + -m10*((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + +m1*(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16))) + +m5*((-((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + *m9) + +((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + *m8 + -((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + *m7 + +m1*(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15))) + +m0*((m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)) + *m9 + -(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)) + *m8 + +(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)) + *m7 + -m10*(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15))) + -m4*(((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m8 + -((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m7 + -m10*((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + +m1*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)))], + [m8*(m13*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m2*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7) + -m9*(m12*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7) + +m10*(m12*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7) + +m1*((-m12*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m14*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7) + -((-m12*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + -m14*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m2*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))) + *m6, + (-m3*(m13*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m2*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7)) + +m4*(m12*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7) + -m5*(m12*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7) + -m0*((-m12*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m14*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m7) + +m1*((-m12*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + -m14*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m2*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))), + m3*(m9*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m1*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m6) + -m4*(m8*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m1*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m6) + +m5*(m8*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m9*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m1*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m6) + +m0*((-m8*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m9*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -m10*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + +(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18)) + *m6) + -m1*(((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m9 + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m8 + -m10*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + +m1*(m15*(m18*m20-m19^2)-m16*(m16*m20-m17*m19) + +m17*(m16*m19-m17*m18))), + (-m3*(m9*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + +m1*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7) + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m6)) + +m4*(m8*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + +m1*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7) + -((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m6) + -m5*(m8*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + -m9*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + +m1*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7) + -((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + *m6) + -m0*((-m8*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7)) + +m9*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7) + +(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18)) + *m6) + +m1*(((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m9 + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m8 + -m10*((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + +m1*(m12*(m18*m20-m19^2)-m13*(m16*m20-m17*m19) + +m14*(m16*m19-m17*m18))), + m3*((m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17)-(m20*m3-m17*m5)*m7) + *m9 + -m10*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m6) + +m5*((-(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + *m9) + +m8*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + -((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + *m6) + +m0*(((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7) + *m9 + -m8*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + +(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17)) + *m6) + -m1*(((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m9 + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m8 + -m10*((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + +m1*(m12*(m16*m20-m17*m19)-m13*(m15*m20-m17^2) + +m14*(m15*m19-m16*m17))) + -m4*(m8*(m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + -m10*(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + +m1*((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7) + -((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m6), + (-m3*(m9*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + +m1*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7) + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m6)) + +m4*(m8*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + +m1*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7) + -((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m6) + -m5*(m8*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + -m9*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + +m1*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7) + -((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + *m6) + -m0*((-m8*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7)) + +m9*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7) + -m10*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7) + +(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)) + *m6) + +m1*(((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m9 + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m8 + -m10*((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + +m1*(m12*(m16*m19-m17*m18)-m13*(m15*m19-m16*m17) + +m14*(m15*m18-m16^2)))], + [(-m7*(m13*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m2*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7)) + +m9*(m11*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m7) + -m10*(m11*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m7) + -m1*((-m11*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m7) + +((-m11*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m2*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))) + *m6, + m2*(m13*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m2*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7) + -m4*(m11*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m7) + +m5*(m11*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m7) + +m0*((-m11*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m13*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m7) + -m1*((-m11*((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3)) + +m13*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m2*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))), + (-m2*(m9*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + +m1*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8) + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m6)) + +m4*(m7*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m6) + -m5*(m7*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m9*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m6) + -m0*((-m7*((-m16*(m20*m9-m10*m19))+m17*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m8)) + +m9*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m10*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + +(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18)) + *m6) + +m1*(((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m9 + -((-m16*(m20*m4-m19*m5))+m17*(m19*m4-m18*m5) + +(m18*m20-m19^2)*m3) + *m7 + -m10*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + +m1*(m12*(m18*m20-m19^2)-m16*(m13*m20-m14*m19) + +m17*(m13*m19-m14*m18))), + m2*(m9*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + +m1*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7) + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m6) + -m4*(m7*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m6) + +m5*(m7*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + -m9*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + -((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + *m6) + +m0*((-m7*((-m13*(m20*m9-m10*m19))+m14*(m19*m9-m10*m18) + +(m18*m20-m19^2)*m7)) + +m9*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -m10*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + +(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18)) + *m6) + -m1*(((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m9 + -((-m13*(m20*m4-m19*m5))+m14*(m19*m4-m18*m5) + +m2*(m18*m20-m19^2)) + *m7 + -m10*((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + +m1*(m11*(m18*m20-m19^2)-m13*(m13*m20-m14*m19) + +m14*(m13*m19-m14*m18))), + (-m2*((m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + *m9 + -m10*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m6)) + -m5*((-(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + *m9) + +m7*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + -((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + *m6) + -m0*(((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + *m9 + -m7*((-m13*(m20*m8-m10*m17))+m14*(m19*m8-m10*m16) + +(m16*m20-m17*m19)*m7) + -m10*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + +(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16)) + *m6) + +m1*(((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m9 + -((-m13*(m20*m3-m17*m5))+m14*(m19*m3-m16*m5) + +m2*(m16*m20-m17*m19)) + *m7 + -m10*((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + +m1*(m11*(m16*m20-m17*m19)-m13*(m12*m20-m14*m17) + +m14*(m12*m19-m14*m16))) + +m4*(m7*(m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + -m10*(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + +m1*((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + -((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m6), + m2*(m9*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + +m1*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7) + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m6) + -m4*(m7*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m6) + +m5*(m7*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + -m9*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + -((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + *m6) + +m0*((-m7*((-m13*(m19*m8-m17*m9))+m14*(m18*m8-m16*m9) + +(m16*m19-m17*m18)*m7)) + +m9*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -m10*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + +(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)) + *m6) + -m1*(((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m9 + -((-m13*(m19*m3-m17*m4))+m14*(m18*m3-m16*m4) + +(m16*m19-m17*m18)*m2) + *m7 + -m10*((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + +m1*(m11*(m16*m19-m17*m18)-m13*(m12*m19-m13*m17) + +m14*(m12*m18-m13*m16)))], + [m7*(m12*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7) + -m8*(m11*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m7) + +m10*(m11*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m12*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m7) + +m1*((-m11*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m12*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m7) + -((-m11*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3)) + +m12*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m2*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))) + *m6, + (-m2*(m12*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7)) + +m3*(m11*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m14*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m7) + -m5*(m11*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m12*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m7) + -m0*((-m11*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m12*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m14*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m7) + +m1*((-m11*((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3)) + +m12*((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + -m14*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m2*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))), + m2*(m8*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m1*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8) + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m6) + -m3*(m7*(m17*(m10*m4-m5*m9)+m3*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m8) + -m10*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m6) + +m5*(m7*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m8*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m6) + +m0*((-m7*((-m15*(m20*m9-m10*m19))+m17*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m8)) + +m8*((-m12*(m20*m9-m10*m19))+m17*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m8) + -m10*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16)) + *m6) + -m1*(((-m12*(m20*m4-m19*m5))+m17*(m14*m4-m13*m5) + +(m13*m20-m14*m19)*m3) + *m8 + -((-m15*(m20*m4-m19*m5))+m17*(m17*m4-m16*m5) + +(m16*m20-m17*m19)*m3) + *m7 + -m10*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +m1*(m12*(m16*m20-m17*m19)-m15*(m13*m20-m14*m19) + +m17*(m13*m17-m14*m16))), + (-m2*(m8*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + +m1*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7) + -((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m6)) + +m3*(m7*(m14*(m10*m4-m5*m9)+m2*(m20*m9-m10*m19) + -(m20*m4-m19*m5)*m7) + -m10*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m6) + -m5*(m7*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + -m8*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + -((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + *m6) + -m0*((-m7*((-m12*(m20*m9-m10*m19))+m14*(m17*m9-m10*m16) + +(m16*m20-m17*m19)*m7)) + +m8*((-m11*(m20*m9-m10*m19))+m14*(m14*m9-m10*m13) + +(m13*m20-m14*m19)*m7) + -m10*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + +(m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16)) + *m6) + +m1*(((-m11*(m20*m4-m19*m5))+m14*(m14*m4-m13*m5) + +m2*(m13*m20-m14*m19)) + *m8 + -((-m12*(m20*m4-m19*m5))+m14*(m17*m4-m16*m5) + +m2*(m16*m20-m17*m19)) + *m7 + -m10*((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + +m1*(m11*(m16*m20-m17*m19)-m12*(m13*m20-m14*m19) + +m14*(m13*m17-m14*m16))), + m2*(m8*(m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + -m10*(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + +m1*((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7) + -((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m6) + -m3*(m7*(m14*(m10*m3-m5*m8)+m2*(m20*m8-m10*m17) + -(m20*m3-m17*m5)*m7) + -m10*(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + +m1*((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + -((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m6) + +m5*(m7*(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + -m8*(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + +m1*((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + -((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + *m6) + +m0*((-m7*((-m12*(m20*m8-m10*m17))+m14*(m17*m8-m10*m15) + +(m15*m20-m17^2)*m7)) + +m8*((-m11*(m20*m8-m10*m17))+m14*(m14*m8-m10*m12) + +(m12*m20-m14*m17)*m7) + -m10*((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + +(m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15)) + *m6) + -m1*(((-m11*(m20*m3-m17*m5))+m14*(m14*m3-m12*m5) + +m2*(m12*m20-m14*m17)) + *m8 + -((-m12*(m20*m3-m17*m5))+m14*(m17*m3-m15*m5) + +m2*(m15*m20-m17^2)) + *m7 + -m10*((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + +m1*(m11*(m15*m20-m17^2)-m12*(m12*m20-m14*m17) + +m14*(m12*m17-m14*m15))), + (-m2*(m8*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + +m1*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7) + -((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m6)) + +m3*(m7*(m14*(m3*m9-m4*m8)+m2*(m19*m8-m17*m9) + -(m19*m3-m17*m4)*m7) + -m10*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m6) + -m5*(m7*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + -m8*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + -((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + *m6) + -m0*((-m7*((-m12*(m19*m8-m17*m9))+m14*(m16*m8-m15*m9) + +(m15*m19-m16*m17)*m7)) + +m8*((-m11*(m19*m8-m17*m9))+m14*(m13*m8-m12*m9) + +(m12*m19-m13*m17)*m7) + -m10*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + +(m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)) + *m6) + +m1*(((-m11*(m19*m3-m17*m4))+m14*(m13*m3-m12*m4) + +(m12*m19-m13*m17)*m2) + *m8 + -((-m12*(m19*m3-m17*m4))+m14*(m16*m3-m15*m4) + +(m15*m19-m16*m17)*m2) + *m7 + -m10*((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + +m1*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m13*m17) + +m14*(m12*m16-m13*m15)))], + [(-m7*(m12*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7)) + +m8*(m11*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m7) + -m9*(m11*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m12*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m7) + -m1*((-m11*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m12*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m13*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m7) + +((-m11*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3)) + +m12*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + -m13*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m2) + *m6, + m2*(m12*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m2*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7) + -m3*(m11*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m13*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m7) + +m4*(m11*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m12*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m2*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m7) + +m0*((-m11*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m12*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m13*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m7) + -m1*((-m11*((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3)) + +m12*((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + -m13*((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m2), + (-m2*(m8*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m9*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + +m1*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8) + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m6)) + +m3*(m7*(m16*(m10*m4-m5*m9)+m3*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m8) + -m9*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m6) + -m4*(m7*(m15*(m10*m4-m5*m9)+m3*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m8) + -m8*(m12*(m10*m4-m5*m9)+m3*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m8) + +m1*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + -((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m6) + -m0*((-m7*((-m15*(m19*m9-m10*m18))+m16*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m8)) + +m8*((-m12*(m19*m9-m10*m18))+m16*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m8) + -m9*((-m12*(m17*m9-m10*m16))+m15*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m8) + +(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16)) + *m6) + +m1*((-((-m12*(m17*m4-m16*m5))+m15*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m3) + *m9) + +((-m12*(m19*m4-m18*m5))+m16*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m3) + *m8 + -((-m15*(m19*m4-m18*m5))+m16*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m3) + *m7 + +m1*(m12*(m16*m19-m17*m18)-m15*(m13*m19-m14*m18) + +m16*(m13*m17-m14*m16))), + m2*(m8*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + -m9*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + +m1*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7) + -((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + *m6) + -m3*(m7*(m13*(m10*m4-m5*m9)+m2*(m19*m9-m10*m18) + -(m19*m4-m18*m5)*m7) + -m9*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + -((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + *m6) + +m4*(m7*(m12*(m10*m4-m5*m9)+m2*(m17*m9-m10*m16) + -(m17*m4-m16*m5)*m7) + -m8*(m11*(m10*m4-m5*m9)+m2*(m14*m9-m10*m13) + -(m14*m4-m13*m5)*m7) + +m1*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + -((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + *m6) + +m0*((-m7*((-m12*(m19*m9-m10*m18))+m13*(m17*m9-m10*m16) + +(m16*m19-m17*m18)*m7)) + +m8*((-m11*(m19*m9-m10*m18))+m13*(m14*m9-m10*m13) + +(m13*m19-m14*m18)*m7) + -m9*((-m11*(m17*m9-m10*m16))+m12*(m14*m9-m10*m13) + +(m13*m17-m14*m16)*m7) + +(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16)) + *m6) + -m1*((-((-m11*(m17*m4-m16*m5))+m12*(m14*m4-m13*m5) + +(m13*m17-m14*m16)*m2) + *m9) + +((-m11*(m19*m4-m18*m5))+m13*(m14*m4-m13*m5) + +(m13*m19-m14*m18)*m2) + *m8 + -((-m12*(m19*m4-m18*m5))+m13*(m17*m4-m16*m5) + +(m16*m19-m17*m18)*m2) + *m7 + +m1*(m11*(m16*m19-m17*m18)-m12*(m13*m19-m14*m18) + +m13*(m13*m17-m14*m16))), + (-m2*((-(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + *m9) + +m8*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + -((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + *m6)) + +m3*((-(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + *m9) + +m7*(m13*(m10*m3-m5*m8)+m2*(m19*m8-m10*m16) + -(m19*m3-m16*m5)*m7) + +m1*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + -((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + *m6) + -m0*((-((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + *m9) + -m7*((-m12*(m19*m8-m10*m16))+m13*(m17*m8-m10*m15) + +(m15*m19-m16*m17)*m7) + +m8*((-m11*(m19*m8-m10*m16))+m13*(m14*m8-m10*m12) + +(m12*m19-m14*m16)*m7) + +(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15)) + *m6) + +m1*((-((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + *m9) + +((-m11*(m19*m3-m16*m5))+m13*(m14*m3-m12*m5) + +(m12*m19-m14*m16)*m2) + *m8 + -((-m12*(m19*m3-m16*m5))+m13*(m17*m3-m15*m5) + +(m15*m19-m16*m17)*m2) + *m7 + +m1*(m11*(m15*m19-m16*m17)-m12*(m12*m19-m14*m16) + +m13*(m12*m17-m14*m15))) + -m4*(m7*(m12*(m10*m3-m5*m8)+m2*(m17*m8-m10*m15) + -(m17*m3-m15*m5)*m7) + -m8*(m11*(m10*m3-m5*m8)+m2*(m14*m8-m10*m12) + -(m14*m3-m12*m5)*m7) + +m1*((-m11*(m17*m8-m10*m15))+m12*(m14*m8-m10*m12) + +(m12*m17-m14*m15)*m7) + -((-m11*(m17*m3-m15*m5))+m12*(m14*m3-m12*m5) + +(m12*m17-m14*m15)*m2) + *m6), + m2*(m8*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + -m9*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + +m1*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7) + -((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + *m6) + -m3*(m7*(m13*(m3*m9-m4*m8)+m2*(m18*m8-m16*m9) + -(m18*m3-m16*m4)*m7) + -m9*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + -((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + *m6) + +m4*(m7*(m12*(m3*m9-m4*m8)+m2*(m16*m8-m15*m9) + -(m16*m3-m15*m4)*m7) + -m8*(m11*(m3*m9-m4*m8)+m2*(m13*m8-m12*m9) + -(m13*m3-m12*m4)*m7) + +m1*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + -((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + *m6) + +m0*((-m7*((-m12*(m18*m8-m16*m9))+m13*(m16*m8-m15*m9) + +(m15*m18-m16^2)*m7)) + +m8*((-m11*(m18*m8-m16*m9))+m13*(m13*m8-m12*m9) + +(m12*m18-m13*m16)*m7) + -m9*((-m11*(m16*m8-m15*m9))+m12*(m13*m8-m12*m9) + +(m12*m16-m13*m15)*m7) + +(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15)) + *m6) + -m1*((-((-m11*(m16*m3-m15*m4))+m12*(m13*m3-m12*m4) + +(m12*m16-m13*m15)*m2) + *m9) + +((-m11*(m18*m3-m16*m4))+m13*(m13*m3-m12*m4) + +(m12*m18-m13*m16)*m2) + *m8 + -((-m12*(m18*m3-m16*m4))+m13*(m16*m3-m15*m4) + +(m15*m18-m16^2)*m2) + *m7 + +m1*(m11*(m15*m18-m16^2)-m12*(m12*m18-m13*m16) + +m13*(m12*m16-m13*m15)))]) + +Test program: + + void main(void) + { + const double a[] = + { 1.9826929 , 0.49539104, 1.21536243, 0.98610923, 1.68623959, 1.0331091 , + 1.96643809, 1.42962549, 0.9336305 , 1.96542156, 0.6086516 , + 0.81542249, 0.74012536, 0.83940333, 1.58604071, + 0.1338364 , 1.03314221, 0.44817192, + 1.97146512, 0.27591278, + 1.51474051 }; + + double c[21]; + + int N=6; + double det = cofactors_sym6(a, c); + + printf("det = %f\n", det); + printf("\n\n"); + + double p[36] = {}; + + for(int iout=0; iout m.*m, m. -> m[.] + +(%i36) determinant(sym2); + +(%o36) m0*m2-m1^2 +(%i37) determinant(sym3); + +(%o37) m0*(m3*m5-m4^2)-m1*(m1*m5-m2*m4)+m2*(m1*m4-m2*m3) +(%i38) determinant(sym4); + +(%o38) m0*(m4*(m7*m9-m8^2)-m5*(m5*m9-m6*m8)+m6*(m5*m8-m6*m7)) + -m1*(m1*(m7*m9-m8^2)-m5*(m2*m9-m3*m8)+m6*(m2*m8-m3*m7)) + +m2*(m1*(m5*m9-m6*m8)-m4*(m2*m9-m3*m8)+m6*(m2*m6-m3*m5)) + -m3*(m1*(m5*m8-m6*m7)-m4*(m2*m8-m3*m7)+m5*(m2*m6-m3*m5)) + +(%i100) determinant(sym5); + +(%o100) -m3*(-m8*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + +m1*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8)+(m10*m14-m11*m13)*m6) + -m5*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4)+(m10*m14-m11*m13)*m2) + +m6*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6)) + +m4*(-m7*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + +m1*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + -m5*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + +m6*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6)) + +m0*(m7*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + -m8*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + +m5*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + -m6*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6)) + -m1*(m7*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + -m8*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + +m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)+(m12*m14-m13^2)*m2) + *m6) + +m2*(m7*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6) + -m8*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6) + +m1*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8)+(m12*m14-m13^2)*m6) + -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)+(m12*m14-m13^2)*m2) + *m5) + +*/ + +#if 0 +#error You really should be using the cofactors functions below to do this. So far Ive only needed the determinant as a part of computing the inverse, and the below functions do this much more efficiently +static inline double det_sym2(const double* m) +{ + return m[0]*m[2]-m[1]*m[1]; +} +static inline double det_sym3(const double* m) +{ + return m[0]*(m[3]*m[5]-m[4]*m[4])-m[1]*(m[1]*m[5]-m[2]*m[4])+m[2]*(m[1]*m[4]-m[2]*m[3]); +} +static inline double det_sym4(const double* m) +{ + return + +m[0]*(m[4]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[5]*m[9]-m[6]*m[8])+m[6]*(m[5]*m[8]-m[6]*m[7])) + -m[1]*(m[1]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[2]*m[9]-m[3]*m[8])+m[6]*(m[2]*m[8]-m[3]*m[7])) + +m[2]*(m[1]*(m[5]*m[9]-m[6]*m[8])-m[4]*(m[2]*m[9]-m[3]*m[8])+m[6]*(m[2]*m[6]-m[3]*m[5])) + -m[3]*(m[1]*(m[5]*m[8]-m[6]*m[7])-m[4]*(m[2]*m[8]-m[3]*m[7])+m[5]*(m[2]*m[6]-m[3]*m[5])); +} +static inline double det_sym5(const double* m) +{ + return + -m[3]*(-m[8]*((m[3]*m[8]-m[4]*m[7])*m[9]+m[2]*(m[11]*m[7]-m[10]*m[8])-(m[11]*m[3]-m[10]*m[4])*m[6]) + +m[1]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6]) + -m[5]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[14]-m[11]*m[13])*m[2]) + +m[6]*(m[11]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[6])) + +m[4]*(-m[7]*((m[3]*m[8]-m[4]*m[7])*m[9]+m[2]*(m[11]*m[7]-m[10]*m[8])-(m[11]*m[3]-m[10]*m[4])*m[6]) + +m[1]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8]) + +(m[10]*m[13]-m[11]*m[12])*m[6]) + -m[5]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4]) + +(m[10]*m[13]-m[11]*m[12])*m[2]) + +m[6]*(m[10]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[6])) + +m[0]*(m[7]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8]) + +(m[10]*m[14]-m[11]*m[13])*m[6]) + -m[8]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8]) + +(m[10]*m[13]-m[11]*m[12])*m[6]) + +m[5]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13]) + +m[11]*(m[10]*m[13]-m[11]*m[12])) + -m[6]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8]) + +(m[12]*m[14]-m[13]*m[13])*m[6])) + -m[1]*(m[7]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4]) + +(m[10]*m[14]-m[11]*m[13])*m[2]) + -m[8]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4]) + +(m[10]*m[13]-m[11]*m[12])*m[2]) + +m[1]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13]) + +m[11]*(m[10]*m[13]-m[11]*m[12])) + -(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2]) + *m[6]) + +m[2]*(m[7]*(m[11]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[6]) + -m[8]*(m[10]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[6]) + +m[1]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6]) + -(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2]) + *m[5]); +} +#endif + +/* I now do inverses. I return transposed cofactors. Original matrix * cofactors / det = identity +As before, I replaced m.^2 -> m.*m, m. -> m[.] +I also removed the lower triangle, since I'm dealing with symmetric matrices here +Session: + + +(%i64) num( ev(invert(sym2),detout) ); + +(%o64) matrix([m2,-m1],[-m1,m0]) +(%i65) num( ev(invert(sym3),detout) ); + +(%o65) matrix([m3*m5-m4^2,m2*m4-m1*m5,m1*m4-m2*m3], + [m2*m4-m1*m5,m0*m5-m2^2,m1*m2-m0*m4], + [m1*m4-m2*m3,m1*m2-m0*m4,m0*m3-m1^2]) +(%i66) num( ev(invert(sym4),detout) ); + +(%o66) matrix([m4*(m7*m9-m8^2)-m5*(m5*m9-m6*m8)+m6*(m5*m8-m6*m7), + -m1*(m7*m9-m8^2)+m2*(m5*m9-m6*m8)-m3*(m5*m8-m6*m7), + m1*(m5*m9-m6*m8)-m2*(m4*m9-m6^2)+m3*(m4*m8-m5*m6), + -m1*(m5*m8-m6*m7)+m2*(m4*m8-m5*m6)-m3*(m4*m7-m5^2)], + [-m1*(m7*m9-m8^2)+m5*(m2*m9-m3*m8)-m6*(m2*m8-m3*m7), + m0*(m7*m9-m8^2)-m2*(m2*m9-m3*m8)+m3*(m2*m8-m3*m7), + -m0*(m5*m9-m6*m8)+m2*(m1*m9-m3*m6)-m3*(m1*m8-m3*m5), + m0*(m5*m8-m6*m7)-m2*(m1*m8-m2*m6)+m3*(m1*m7-m2*m5)], + [m1*(m5*m9-m6*m8)-m4*(m2*m9-m3*m8)+m6*(m2*m6-m3*m5), + -m0*(m5*m9-m6*m8)+m1*(m2*m9-m3*m8)-m3*(m2*m6-m3*m5), + m0*(m4*m9-m6^2)-m1*(m1*m9-m3*m6)+m3*(m1*m6-m3*m4), + -m0*(m4*m8-m5*m6)+m1*(m1*m8-m2*m6)-m3*(m1*m5-m2*m4)], + [-m1*(m5*m8-m6*m7)+m4*(m2*m8-m3*m7)-m5*(m2*m6-m3*m5), + m0*(m5*m8-m6*m7)-m1*(m2*m8-m3*m7)+m2*(m2*m6-m3*m5), + -m0*(m4*m8-m5*m6)+m1*(m1*m8-m3*m5)-m2*(m1*m6-m3*m4), + m0*(m4*m7-m5^2)-m1*(m1*m7-m2*m5)+m2*(m1*m5-m2*m4)]) +(%i103) num( ev(invert(sym5),detout) ); + +(%o103) matrix([m7*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + -m8*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + +m5*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + -m6*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6), + -m3*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + +m4*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + -m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + +m2*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6), + -m2*(-m7*(m14*m7-m13*m8)+m8*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m5) + +m3*(-m6*(m14*m7-m13*m8)+m8*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m5) + -m4*(-m6*(m13*m7-m12*m8)+m7*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m5) + +m1*((m10*m13-m11*m12)*m8-(m10*m14-m11*m13)*m7 + +(m12*m14-m13^2)*m6), + -m3*(m8*(m11*m6-m8*m9)+m5*(m14*m9-m11^2)-m6*(m14*m6-m11*m8)) + +m4*(m7*(m11*m6-m8*m9)+m5*(m13*m9-m10*m11) + -m6*(m13*m6-m10*m8)) + -m1*(-m7*(m14*m9-m11^2)+m8*(m13*m9-m10*m11) + +(m10*m14-m11*m13)*m6) + +m2*(-m7*(m14*m6-m11*m8)+m8*(m13*m6-m10*m8) + +(m10*m14-m11*m13)*m5), + m3*(m8*(m10*m6-m7*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m11*m7)) + -m4*(m7*(m10*m6-m7*m9)+m5*(m12*m9-m10^2)-m6*(m12*m6-m10*m7)) + +m1*(-m7*(m13*m9-m10*m11)+m8*(m12*m9-m10^2) + +(m10*m13-m11*m12)*m6) + -m2*((m12*m6-m10*m7)*m8-m7*(m13*m6-m11*m7) + +(m10*m13-m11*m12)*m5)], + [-m7*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + +m8*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + -m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + +(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4) + +(m12*m14-m13^2)*m2) + *m6, + m3*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + -m4*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + +m0*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13) + +m11*(m10*m13-m11*m12)) + -m2*(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4) + +(m12*m14-m13^2)*m2), + m2*((m13*m3-m12*m4)*m8-(m14*m3-m13*m4)*m7+m1*(m12*m14-m13^2)) + -m3*((m11*m3-m10*m4)*m8-(m14*m3-m13*m4)*m6 + +m1*(m10*m14-m11*m13)) + -m0*((m10*m13-m11*m12)*m8-(m10*m14-m11*m13)*m7 + +(m12*m14-m13^2)*m6) + +m4*((m11*m3-m10*m4)*m7-(m13*m3-m12*m4)*m6 + +m1*(m10*m13-m11*m12)), + m3*(m8*(m11*m2-m4*m9)+m1*(m14*m9-m11^2)-(m14*m2-m11*m4)*m6) + -m4*(m7*(m11*m2-m4*m9)+m1*(m13*m9-m10*m11) + -(m13*m2-m10*m4)*m6) + +m0*(-m7*(m14*m9-m11^2)+m8*(m13*m9-m10*m11) + +(m10*m14-m11*m13)*m6) + -m2*((m13*m2-m10*m4)*m8-(m14*m2-m11*m4)*m7 + +m1*(m10*m14-m11*m13)), + -m3*(m8*(m10*m2-m3*m9)+m1*(m13*m9-m10*m11)-(m13*m2-m11*m3)*m6) + +m4*(m7*(m10*m2-m3*m9)+m1*(m12*m9-m10^2)-(m12*m2-m10*m3)*m6) + -m0*(-m7*(m13*m9-m10*m11)+m8*(m12*m9-m10^2) + +(m10*m13-m11*m12)*m6) + +m2*((m12*m2-m10*m3)*m8-(m13*m2-m11*m3)*m7 + +m1*(m10*m13-m11*m12))], + [m7*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6) + -m8*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6) + +m1*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6) + -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4) + +(m12*m14-m13^2)*m2) + *m5, + -m3*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6) + +m4*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6) + -m0*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m6) + +m1*(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4) + +(m12*m14-m13^2)*m2), + m3*(m8*(m3*m8-m4*m7)+m1*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m5) + -m4*(m7*(m3*m8-m4*m7)+m1*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m5) + +m0*(-m7*(m14*m7-m13*m8)+m8*(m13*m7-m12*m8) + +(m12*m14-m13^2)*m5) + -m1*((m13*m3-m12*m4)*m8-(m14*m3-m13*m4)*m7 + +m1*(m12*m14-m13^2)), + -m3*(m8*(m2*m8-m4*m6)+m1*(m14*m6-m11*m8)-(m14*m2-m11*m4)*m5) + +m4*(m7*(m2*m8-m4*m6)+m1*(m13*m6-m10*m8)-(m13*m2-m10*m4)*m5) + -m0*(-m7*(m14*m6-m11*m8)+m8*(m13*m6-m10*m8) + +(m10*m14-m11*m13)*m5) + +m1*((m13*m2-m10*m4)*m8-(m14*m2-m11*m4)*m7 + +m1*(m10*m14-m11*m13)), + m3*((m2*m7-m3*m6)*m8+m1*(m13*m6-m11*m7)-(m13*m2-m11*m3)*m5) + +m0*((m12*m6-m10*m7)*m8-m7*(m13*m6-m11*m7) + +(m10*m13-m11*m12)*m5) + -m1*((m12*m2-m10*m3)*m8-(m13*m2-m11*m3)*m7 + +m1*(m10*m13-m11*m12)) + -m4*(m7*(m2*m7-m3*m6)+m1*(m12*m6-m10*m7) + -(m12*m2-m10*m3)*m5)], + [m8*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + -m1*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + +m5*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + -m6*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8) + -(m14*m3-m13*m4)*m6), + -m4*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + +m0*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m6) + -m1*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4) + +(m10*m14-m11*m13)*m2) + +m2*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8) + -(m14*m3-m13*m4)*m6), + -m2*(m8*(m3*m8-m4*m7)+m1*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m5) + +m4*(m6*(m3*m8-m4*m7)+m1*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m5) + -m0*(-m6*(m14*m7-m13*m8)+m8*(m11*m7-m10*m8) + +(m10*m14-m11*m13)*m5) + +m1*((m11*m3-m10*m4)*m8-(m14*m3-m13*m4)*m6 + +m1*(m10*m14-m11*m13)), + m0*(m8*(m11*m6-m8*m9)+m5*(m14*m9-m11^2)-m6*(m14*m6-m11*m8)) + -m4*(m1*(m11*m6-m8*m9)-m5*(m11*m2-m4*m9)+m6*(m2*m8-m4*m6)) + -m1*(m8*(m11*m2-m4*m9)+m1*(m14*m9-m11^2)-(m14*m2-m11*m4)*m6) + +m2*(m8*(m2*m8-m4*m6)+m1*(m14*m6-m11*m8)-(m14*m2-m11*m4)*m5), + -m0*(m8*(m10*m6-m7*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m11*m7)) + +m4*(m1*(m10*m6-m7*m9)-m5*(m10*m2-m3*m9)+m6*(m2*m7-m3*m6)) + +m1*(m8*(m10*m2-m3*m9)+m1*(m13*m9-m10*m11) + -(m13*m2-m11*m3)*m6) + -m2*((m2*m7-m3*m6)*m8+m1*(m13*m6-m11*m7) + -(m13*m2-m11*m3)*m5)], + [-m7*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + +m1*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + -m5*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + +m6*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8) + -(m13*m3-m12*m4)*m6), + m3*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6) + -m0*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m6) + +m1*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4) + +(m10*m13-m11*m12)*m2) + -m2*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8) + -(m13*m3-m12*m4)*m6), + m2*(m7*(m3*m8-m4*m7)+m1*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m5) + -m3*(m6*(m3*m8-m4*m7)+m1*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m5) + +m0*(-m6*(m13*m7-m12*m8)+m7*(m11*m7-m10*m8) + +(m10*m13-m11*m12)*m5) + -m1*((m11*m3-m10*m4)*m7-(m13*m3-m12*m4)*m6 + +m1*(m10*m13-m11*m12)), + -m0*(m7*(m11*m6-m8*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m10*m8)) + +m3*(m1*(m11*m6-m8*m9)-m5*(m11*m2-m4*m9)+m6*(m2*m8-m4*m6)) + +m1*(m7*(m11*m2-m4*m9)+m1*(m13*m9-m10*m11) + -(m13*m2-m10*m4)*m6) + -m2*(m7*(m2*m8-m4*m6)+m1*(m13*m6-m10*m8)-(m13*m2-m10*m4)*m5), + m0*(m7*(m10*m6-m7*m9)+m5*(m12*m9-m10^2)-m6*(m12*m6-m10*m7)) + -m3*(m1*(m10*m6-m7*m9)-m5*(m10*m2-m3*m9)+m6*(m2*m7-m3*m6)) + -m1*(m7*(m10*m2-m3*m9)+m1*(m12*m9-m10^2)-(m12*m2-m10*m3)*m6) + +m2*(m7*(m2*m7-m3*m6)+m1*(m12*m6-m10*m7) + -(m12*m2-m10*m3)*m5)]) +*/ + +static inline double cofactors_sym2(const double* restrict m, double* restrict c) +{ + c[0] = m[2]; + c[1] = -m[1]; + c[2] = -m[1]; + + return m[0]*c[0] + m[1]*c[1]; +} + +static inline double cofactors_sym3(const double* restrict m, double* restrict c) +{ + c[0] = m[3]*m[5]-m[4]*m[4]; + c[1] = m[2]*m[4]-m[1]*m[5]; + c[2] = m[1]*m[4]-m[2]*m[3]; + c[3] = m[0]*m[5]-m[2]*m[2]; + c[4] = m[1]*m[2]-m[0]*m[4]; + c[5] = m[0]*m[3]-m[1]*m[1]; + + return m[0]*c[0] + m[1]*c[1] + m[2]*c[2]; +} + +static inline double cofactors_sym4(const double* restrict m, double* restrict c) +{ + c[0] = m[4]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[5]*m[9]-m[6]*m[8])+m[6]*(m[5]*m[8]-m[6]*m[7]); + c[1] = -m[1]*(m[7]*m[9]-m[8]*m[8])+m[2]*(m[5]*m[9]-m[6]*m[8])-m[3]*(m[5]*m[8]-m[6]*m[7]); + c[2] = m[1]*(m[5]*m[9]-m[6]*m[8])-m[2]*(m[4]*m[9]-m[6]*m[6])+m[3]*(m[4]*m[8]-m[5]*m[6]); + c[3] = -m[1]*(m[5]*m[8]-m[6]*m[7])+m[2]*(m[4]*m[8]-m[5]*m[6])-m[3]*(m[4]*m[7]-m[5]*m[5]); + c[4] = m[0]*(m[7]*m[9]-m[8]*m[8])-m[2]*(m[2]*m[9]-m[3]*m[8])+m[3]*(m[2]*m[8]-m[3]*m[7]); + c[5] = -m[0]*(m[5]*m[9]-m[6]*m[8])+m[2]*(m[1]*m[9]-m[3]*m[6])-m[3]*(m[1]*m[8]-m[3]*m[5]); + c[6] = m[0]*(m[5]*m[8]-m[6]*m[7])-m[2]*(m[1]*m[8]-m[2]*m[6])+m[3]*(m[1]*m[7]-m[2]*m[5]); + c[7] = m[0]*(m[4]*m[9]-m[6]*m[6])-m[1]*(m[1]*m[9]-m[3]*m[6])+m[3]*(m[1]*m[6]-m[3]*m[4]); + c[8] = -m[0]*(m[4]*m[8]-m[5]*m[6])+m[1]*(m[1]*m[8]-m[2]*m[6])-m[3]*(m[1]*m[5]-m[2]*m[4]); + c[9] = m[0]*(m[4]*m[7]-m[5]*m[5])-m[1]*(m[1]*m[7]-m[2]*m[5])+m[2]*(m[1]*m[5]-m[2]*m[4]); + + return m[0]*c[0] + m[1]*c[1] + m[2]*c[2] + m[3]*c[3]; +} + +static inline double cofactors_sym5(const double* restrict m, double* restrict c) +{ + c[0] = m[7]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6])-m[8]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[6])+m[5]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))-m[6]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6]); + c[1] = -m[3]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6])+m[4]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[6])-m[1]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))+m[2]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6]); + c[2] = -m[2]*(-m[7]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[5])+m[3]*(-m[6]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5])-m[4]*(-m[6]*(m[13]*m[7]-m[12]*m[8])+m[7]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[5])+m[1]*((m[10]*m[13]-m[11]*m[12])*m[8]-(m[10]*m[14]-m[11]*m[13])*m[7]+(m[12]*m[14]-m[13]*m[13])*m[6]); + c[3] = -m[3]*(m[8]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[14]*m[9]-m[11]*m[11])-m[6]*(m[14]*m[6]-m[11]*m[8]))+m[4]*(m[7]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[10]*m[8]))-m[1]*(-m[7]*(m[14]*m[9]-m[11]*m[11])+m[8]*(m[13]*m[9]-m[10]*m[11])+(m[10]*m[14]-m[11]*m[13])*m[6])+m[2]*(-m[7]*(m[14]*m[6]-m[11]*m[8])+m[8]*(m[13]*m[6]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5]); + c[4] = m[3]*(m[8]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[11]*m[7]))-m[4]*(m[7]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[12]*m[9]-m[10]*m[10])-m[6]*(m[12]*m[6]-m[10]*m[7]))+m[1]*(-m[7]*(m[13]*m[9]-m[10]*m[11])+m[8]*(m[12]*m[9]-m[10]*m[10])+(m[10]*m[13]-m[11]*m[12])*m[6])-m[2]*((m[12]*m[6]-m[10]*m[7])*m[8]-m[7]*(m[13]*m[6]-m[11]*m[7])+(m[10]*m[13]-m[11]*m[12])*m[5]); + c[5] = m[3]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[14]-m[11]*m[13])*m[2])-m[4]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[13]-m[11]*m[12])*m[2])+m[0]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))-m[2]*(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2]); + c[6] = m[2]*((m[13]*m[3]-m[12]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[7]+m[1]*(m[12]*m[14]-m[13]*m[13]))-m[3]*((m[11]*m[3]-m[10]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[6]+m[1]*(m[10]*m[14]-m[11]*m[13]))-m[0]*((m[10]*m[13]-m[11]*m[12])*m[8]-(m[10]*m[14]-m[11]*m[13])*m[7]+(m[12]*m[14]-m[13]*m[13])*m[6])+m[4]*((m[11]*m[3]-m[10]*m[4])*m[7]-(m[13]*m[3]-m[12]*m[4])*m[6]+m[1]*(m[10]*m[13]-m[11]*m[12])); + c[7] = m[3]*(m[8]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[14]*m[9]-m[11]*m[11])-(m[14]*m[2]-m[11]*m[4])*m[6])-m[4]*(m[7]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[10]*m[4])*m[6])+m[0]*(-m[7]*(m[14]*m[9]-m[11]*m[11])+m[8]*(m[13]*m[9]-m[10]*m[11])+(m[10]*m[14]-m[11]*m[13])*m[6])-m[2]*((m[13]*m[2]-m[10]*m[4])*m[8]-(m[14]*m[2]-m[11]*m[4])*m[7]+m[1]*(m[10]*m[14]-m[11]*m[13])); + c[8] = -m[3]*(m[8]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[11]*m[3])*m[6])+m[4]*(m[7]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[12]*m[9]-m[10]*m[10])-(m[12]*m[2]-m[10]*m[3])*m[6])-m[0]*(-m[7]*(m[13]*m[9]-m[10]*m[11])+m[8]*(m[12]*m[9]-m[10]*m[10])+(m[10]*m[13]-m[11]*m[12])*m[6])+m[2]*((m[12]*m[2]-m[10]*m[3])*m[8]-(m[13]*m[2]-m[11]*m[3])*m[7]+m[1]*(m[10]*m[13]-m[11]*m[12])); + c[9] = m[3]*(m[8]*(m[3]*m[8]-m[4]*m[7])+m[1]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[5])-m[4]*(m[7]*(m[3]*m[8]-m[4]*m[7])+m[1]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[5])+m[0]*(-m[7]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[5])-m[1]*((m[13]*m[3]-m[12]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[7]+m[1]*(m[12]*m[14]-m[13]*m[13])); + c[10] = -m[3]*(m[8]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[14]*m[6]-m[11]*m[8])-(m[14]*m[2]-m[11]*m[4])*m[5])+m[4]*(m[7]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[13]*m[6]-m[10]*m[8])-(m[13]*m[2]-m[10]*m[4])*m[5])-m[0]*(-m[7]*(m[14]*m[6]-m[11]*m[8])+m[8]*(m[13]*m[6]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5])+m[1]*((m[13]*m[2]-m[10]*m[4])*m[8]-(m[14]*m[2]-m[11]*m[4])*m[7]+m[1]*(m[10]*m[14]-m[11]*m[13])); + c[11] = m[3]*((m[2]*m[7]-m[3]*m[6])*m[8]+m[1]*(m[13]*m[6]-m[11]*m[7])-(m[13]*m[2]-m[11]*m[3])*m[5])+m[0]*((m[12]*m[6]-m[10]*m[7])*m[8]-m[7]*(m[13]*m[6]-m[11]*m[7])+(m[10]*m[13]-m[11]*m[12])*m[5])-m[1]*((m[12]*m[2]-m[10]*m[3])*m[8]-(m[13]*m[2]-m[11]*m[3])*m[7]+m[1]*(m[10]*m[13]-m[11]*m[12]))-m[4]*(m[7]*(m[2]*m[7]-m[3]*m[6])+m[1]*(m[12]*m[6]-m[10]*m[7])-(m[12]*m[2]-m[10]*m[3])*m[5]); + c[12] = m[0]*(m[8]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[14]*m[9]-m[11]*m[11])-m[6]*(m[14]*m[6]-m[11]*m[8]))-m[4]*(m[1]*(m[11]*m[6]-m[8]*m[9])-m[5]*(m[11]*m[2]-m[4]*m[9])+m[6]*(m[2]*m[8]-m[4]*m[6]))-m[1]*(m[8]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[14]*m[9]-m[11]*m[11])-(m[14]*m[2]-m[11]*m[4])*m[6])+m[2]*(m[8]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[14]*m[6]-m[11]*m[8])-(m[14]*m[2]-m[11]*m[4])*m[5]); + c[13] = -m[0]*(m[8]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[11]*m[7]))+m[4]*(m[1]*(m[10]*m[6]-m[7]*m[9])-m[5]*(m[10]*m[2]-m[3]*m[9])+m[6]*(m[2]*m[7]-m[3]*m[6]))+m[1]*(m[8]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[11]*m[3])*m[6])-m[2]*((m[2]*m[7]-m[3]*m[6])*m[8]+m[1]*(m[13]*m[6]-m[11]*m[7])-(m[13]*m[2]-m[11]*m[3])*m[5]); + c[14] = m[0]*(m[7]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[12]*m[9]-m[10]*m[10])-m[6]*(m[12]*m[6]-m[10]*m[7]))-m[3]*(m[1]*(m[10]*m[6]-m[7]*m[9])-m[5]*(m[10]*m[2]-m[3]*m[9])+m[6]*(m[2]*m[7]-m[3]*m[6]))-m[1]*(m[7]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[12]*m[9]-m[10]*m[10])-(m[12]*m[2]-m[10]*m[3])*m[6])+m[2]*(m[7]*(m[2]*m[7]-m[3]*m[6])+m[1]*(m[12]*m[6]-m[10]*m[7])-(m[12]*m[2]-m[10]*m[3])*m[5]); + + return m[0]*c[0] + m[1]*c[1] + m[2]*c[2] + m[3]*c[3] + m[4]*c[4]; +} + +/* +The upper-triangular and lower-triangular routines have a similar API to the +symmetric ones. Note that as with symmetric matrices, we don't store redundant +data, and we store data row-first. So the upper-triangular matrices have N +elements in the first row in memory, but lower-triangular matrices have only 1. + +Inverses of triangular matrices are similarly triangular, and that's how I store +them + + +Session: + +// upper triangular +(%i2) ut2 : matrix([m0,m1],[0,m2]); + +(%o2) matrix([m0,m1],[0,m2]) +(%i3) ut3 : matrix([m0,m1,m2],[0,m3,m4],[0,0,m5]); + +(%o3) matrix([m0,m1,m2],[0,m3,m4],[0,0,m5]) +(%i4) ut4 : matrix([m0,m1,m2,m3],[0,m4,m5,m6],[0,0,m7,m8],[0,0,0,m9]); + +(%o4) matrix([m0,m1,m2,m3],[0,m4,m5,m6],[0,0,m7,m8],[0,0,0,m9]) +(%i5) ut5 : matrix([m0,m1,m2,m3,m4],[0,m5,m6,m7,m8],[0,0,m9,m10,m11],[0,0,0,m12,m13],[0,0,0,0,m14]); + +(%o5) matrix([m0,m1,m2,m3,m4],[0,m5,m6,m7,m8],[0,0,m9,m10,m11], + [0,0,0,m12,m13],[0,0,0,0,m14]) + +(%i11) num( ev(invert(ut2),detout) ); + +(%o11) matrix([m2,-m1],[0,m0]) +(%i12) num( ev(invert(ut3),detout) ); + +(%o12) matrix([m3*m5,-m1*m5,m1*m4-m2*m3],[0,m0*m5,-m0*m4],[0,0,m0*m3]) +(%i13) num( ev(invert(ut4),detout) ); + +(%o13) matrix([m4*m7*m9,-m1*m7*m9,m1*m5*m9-m2*m4*m9, + (-m1*(m5*m8-m6*m7))+m2*m4*m8-m3*m4*m7], + [0,m0*m7*m9,-m0*m5*m9,m0*(m5*m8-m6*m7)], + [0,0,m0*m4*m9,-m0*m4*m8],[0,0,0,m0*m4*m7]) +(%i14) num( ev(invert(ut5),detout) ); + +(%o14) matrix([m12*m14*m5*m9,-m1*m12*m14*m9,m1*m12*m14*m6-m12*m14*m2*m5, + (-m1*(m10*m14*m6-m14*m7*m9))-m14*m3*m5*m9+m10*m14*m2*m5, + m1*(m12*m8*m9-m13*m7*m9+(m10*m13-m11*m12)*m6) + -m12*m4*m5*m9+m13*m3*m5*m9-(m10*m13-m11*m12)*m2*m5], + [0,m0*m12*m14*m9,-m0*m12*m14*m6,m0*(m10*m14*m6-m14*m7*m9), + -m0*(m12*m8*m9-m13*m7*m9+(m10*m13-m11*m12)*m6)], + [0,0,m0*m12*m14*m5,-m0*m10*m14*m5,m0*(m10*m13-m11*m12)*m5], + [0,0,0,m0*m14*m5*m9,-m0*m13*m5*m9],[0,0,0,0,m0*m12*m5*m9]) + + +// lower-triangular +(%i19) lt2 : matrix([m0,0],[m1,m2]); + +(%o19) matrix([m0,0],[m1,m2]) +(%i20) lt3 : matrix([m0,0,0],[m1,m2,0],[m3,m4,m5]); + +(%o20) matrix([m0,0,0],[m1,m2,0],[m3,m4,m5]) +(%i21) lt4 : matrix([m0,0,0,0],[m1,m2,0,0],[m3,m4,m5,0],[m6,m7,m8,m9]); + +(%o21) matrix([m0,0,0,0],[m1,m2,0,0],[m3,m4,m5,0],[m6,m7,m8,m9]) +(%i22) lt5 : matrix([m0,0,0,0,0],[m1,m2,0,0,0],[m3,m4,m5,0,0],[m6,m7,m8,m9,0],[m10,m11,m12,m13,m14]); + +(%o22) matrix([m0,0,0,0,0],[m1,m2,0,0,0],[m3,m4,m5,0,0],[m6,m7,m8,m9,0], + [m10,m11,m12,m13,m14]) +(%i23) num( ev(invert(lt2),detout) ); + +(%o23) matrix([m2,0],[-m1,m0]) +(%i24) num( ev(invert(lt3),detout) ); + +(%o24) matrix([m2*m5,0,0],[-m1*m5,m0*m5,0],[m1*m4-m2*m3,-m0*m4,m0*m2]) +(%i25) num( ev(invert(lt4),detout) ); + +(%o25) matrix([m2*m5*m9,0,0,0],[-m1*m5*m9,m0*m5*m9,0,0], + [m1*m4*m9-m2*m3*m9,-m0*m4*m9,m0*m2*m9,0], + [m2*(m3*m8-m5*m6)-m1*(m4*m8-m5*m7),m0*(m4*m8-m5*m7),-m0*m2*m8, + m0*m2*m5]) +(%i26) num( ev(invert(lt5),detout) ); + +(%o26) matrix([m14*m2*m5*m9,0,0,0,0],[-m1*m14*m5*m9,m0*m14*m5*m9,0,0,0], + [m1*m14*m4*m9-m14*m2*m3*m9,-m0*m14*m4*m9,m0*m14*m2*m9,0,0], + [m2*(m14*m3*m8-m14*m5*m6)-m1*(m14*m4*m8-m14*m5*m7), + m0*(m14*m4*m8-m14*m5*m7),-m0*m14*m2*m8,m0*m14*m2*m5,0], + [m1*(m4*(m13*m8-m12*m9)-m5*(m13*m7-m11*m9)) + -m2*(m3*(m13*m8-m12*m9)-m5*(m13*m6-m10*m9)), + -m0*(m4*(m13*m8-m12*m9)-m5*(m13*m7-m11*m9)), + m0*m2*(m13*m8-m12*m9),-m0*m13*m2*m5,m0*m2*m5*m9]) + + */ +static inline double cofactors_ut2(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[2]; + c[i++] = -m[1]; + c[i++] = m[0]; + return m[0]*m[2]; +} +static inline double cofactors_ut3(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[3]*m[5]; + c[i++] = -m[1]*m[5]; + c[i++] = m[1]*m[4]-m[2]*m[3]; + c[i++] = m[0]*m[5]; + c[i++] = -m[0]*m[4]; + c[i++] = m[0]*m[3]; + return m[0]*m[3]*m[5]; +} +static inline double cofactors_ut4(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[4]*m[7]*m[9]; + c[i++] = -m[1]*m[7]*m[9]; + c[i++] = m[1]*m[5]*m[9]-m[2]*m[4]*m[9]; + c[i++] = (-m[1]*(m[5]*m[8]-m[6]*m[7]))+m[2]*m[4]*m[8]-m[3]*m[4]*m[7]; + c[i++] = m[0]*m[7]*m[9]; + c[i++] = -m[0]*m[5]*m[9]; + c[i++] = m[0]*(m[5]*m[8]-m[6]*m[7]); + c[i++] = m[0]*m[4]*m[9]; + c[i++] = -m[0]*m[4]*m[8]; + c[i++] = m[0]*m[4]*m[7]; + return m[0]*m[4]*m[7]*m[9]; +} +static inline double cofactors_ut5(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[12]*m[14]*m[5]*m[9]; + c[i++] = -m[1]*m[12]*m[14]*m[9]; + c[i++] = m[1]*m[12]*m[14]*m[6]-m[12]*m[14]*m[2]*m[5]; + c[i++] = (-m[1]*(m[10]*m[14]*m[6]-m[14]*m[7]*m[9]))-m[14]*m[3]*m[5]*m[9]+m[10]*m[14]*m[2]*m[5]; + c[i++] = m[1]*(m[12]*m[8]*m[9]-m[13]*m[7]*m[9]+(m[10]*m[13]-m[11]*m[12])*m[6])-m[12]*m[4]*m[5]*m[9]+m[13]*m[3]*m[5]*m[9]-(m[10]*m[13]-m[11]*m[12])*m[2]*m[5]; + c[i++] = m[0]*m[12]*m[14]*m[9]; + c[i++] = -m[0]*m[12]*m[14]*m[6]; + c[i++] = m[0]*(m[10]*m[14]*m[6]-m[14]*m[7]*m[9]); + c[i++] = -m[0]*(m[12]*m[8]*m[9]-m[13]*m[7]*m[9]+(m[10]*m[13]-m[11]*m[12])*m[6]); + c[i++] = m[0]*m[12]*m[14]*m[5]; + c[i++] = -m[0]*m[10]*m[14]*m[5]; + c[i++] = m[0]*(m[10]*m[13]-m[11]*m[12])*m[5]; + c[i++] = m[0]*m[14]*m[5]*m[9]; + c[i++] = -m[0]*m[13]*m[5]*m[9]; + c[i++] = m[0]*m[12]*m[5]*m[9]; + return m[0]*m[5]*m[9]*m[12]*m[14]; +} +static inline double cofactors_lt2(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[2]; + c[i++] = -m[1]; + c[i++] = m[0]; + return m[0]*m[2]; +} +static inline double cofactors_lt3(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[2]*m[5]; + c[i++] = -m[1]*m[5]; + c[i++] = m[0]*m[5]; + c[i++] = m[1]*m[4]-m[2]*m[3]; + c[i++] = -m[0]*m[4]; + c[i++] = m[0]*m[2]; + return m[0]*m[2]*m[5]; +} +static inline double cofactors_lt4(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[2]*m[5]*m[9]; + c[i++] = -m[1]*m[5]*m[9]; + c[i++] = m[0]*m[5]*m[9]; + c[i++] = m[1]*m[4]*m[9]-m[2]*m[3]*m[9]; + c[i++] = -m[0]*m[4]*m[9]; + c[i++] = m[0]*m[2]*m[9]; + c[i++] = m[2]*(m[3]*m[8]-m[5]*m[6])-m[1]*(m[4]*m[8]-m[5]*m[7]); + c[i++] = m[0]*(m[4]*m[8]-m[5]*m[7]); + c[i++] = -m[0]*m[2]*m[8]; + c[i++] = m[0]*m[2]*m[5]; + return m[0]*m[2]*m[5]*m[9]; +} +static inline double cofactors_lt5(const double* restrict m, double* restrict c) +{ + int i=0; + c[i++] = m[14]*m[2]*m[5]*m[9]; + c[i++] = -m[1]*m[14]*m[5]*m[9]; + c[i++] = m[0]*m[14]*m[5]*m[9]; + c[i++] = m[1]*m[14]*m[4]*m[9]-m[14]*m[2]*m[3]*m[9]; + c[i++] = -m[0]*m[14]*m[4]*m[9]; + c[i++] = m[0]*m[14]*m[2]*m[9]; + c[i++] = m[2]*(m[14]*m[3]*m[8]-m[14]*m[5]*m[6])-m[1]*(m[14]*m[4]*m[8]-m[14]*m[5]*m[7]); + c[i++] = m[0]*(m[14]*m[4]*m[8]-m[14]*m[5]*m[7]); + c[i++] = -m[0]*m[14]*m[2]*m[8]; + c[i++] = m[0]*m[14]*m[2]*m[5]; + c[i++] = m[1]*(m[4]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[7]-m[11]*m[9]))-m[2]*(m[3]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[6]-m[10]*m[9])); + c[i++] = -m[0]*(m[4]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[7]-m[11]*m[9])); + c[i++] = m[0]*m[2]*(m[13]*m[8]-m[12]*m[9]); + c[i++] = -m[0]*m[13]*m[2]*m[5]; + c[i++] = m[0]*m[2]*m[5]*m[9]; + return m[0]*m[2]*m[5]*m[9]*m[14]; +} + +/* +(%i27) a : matrix([a0,a1,a2],[0,a3,a4],[0,0,a5]); + +(%o27) matrix([a0,a1,a2],[0,a3,a4],[0,0,a5]) +(%i28) b : matrix([b0,b1,b2],[0,b3,b4],[0,0,b5]); + +(%o28) matrix([b0,b1,b2],[0,b3,b4],[0,0,b5]) +(%i29) a . b; + +(%o29) matrix([a0*b0,a1*b3+a0*b1,a2*b5+a1*b4+a0*b2],[0,a3*b3,a4*b5+a3*b4],[0,0,a5*b5]) +*/ +static inline void mul_ut3_ut3(const double* restrict a, const double* restrict b, + double* restrict ab) +{ + ab[0] = a[0] * b[0]; + ab[1] = a[1] * b[3]+a[0] * b[1]; + ab[2] = a[2] * b[5]+a[1] * b[4]+a[0] * b[2]; + ab[3] = a[3] * b[3]; + ab[4] = a[4] * b[5]+a[3] * b[4]; + ab[5] = a[5] * b[5]; +} + +// symmetrix 3x3 by symmetrix 3x3, written into a new non-symmetric matrix, +// scaled. This is a special-case function that I needed for something... +static inline void mul_sym33_sym33_scaled_out(const double* restrict s0, const double* restrict s1, double* restrict mout, double scale) +{ +// (%i106) matrix([m0_0,m0_1,m0_2], +// [m0_1,m0_3,m0_4], +// [m0_2,m0_4,m0_5]) . +// matrix([m1_0,m1_1,m1_2], +// [m1_1,m1_3,m1_4], +// [m1_2,m1_4,m1_5]); + +// (%o106) matrix([m0_2*m1_2+m0_1*m1_1+m0_0*m1_0,m0_2*m1_4+m0_1*m1_3+m0_0*m1_1, +// m0_2*m1_5+m0_1*m1_4+m0_0*m1_2], +// [m0_4*m1_2+m0_3*m1_1+m0_1*m1_0,m0_4*m1_4+m0_3*m1_3+m0_1*m1_1, +// m0_4*m1_5+m0_3*m1_4+m0_1*m1_2], +// [m0_5*m1_2+m0_4*m1_1+m0_2*m1_0,m0_5*m1_4+m0_4*m1_3+m0_2*m1_1, +// m0_5*m1_5+m0_4*m1_4+m0_2*m1_2]) + + mout[0] = scale * (s0[2]*s1[2]+s0[1]*s1[1]+s0[0]*s1[0]); + mout[1] = scale * (s0[2]*s1[4]+s0[1]*s1[3]+s0[0]*s1[1]); + mout[2] = scale * (s0[2]*s1[5]+s0[1]*s1[4]+s0[0]*s1[2]); + mout[3] = scale * (s0[4]*s1[2]+s0[3]*s1[1]+s0[1]*s1[0]); + mout[4] = scale * (s0[4]*s1[4]+s0[3]*s1[3]+s0[1]*s1[1]); + mout[5] = scale * (s0[4]*s1[5]+s0[3]*s1[4]+s0[1]*s1[2]); + mout[6] = scale * (s0[5]*s1[2]+s0[4]*s1[1]+s0[2]*s1[0]); + mout[7] = scale * (s0[5]*s1[4]+s0[4]*s1[3]+s0[2]*s1[1]); + mout[8] = scale * (s0[5]*s1[5]+s0[4]*s1[4]+s0[2]*s1[2]); +} + +static inline void outerproduct3(const double* restrict v, double* restrict P) +{ + P[0] = v[0]*v[0]; + P[1] = v[0]*v[1]; + P[2] = v[0]*v[2]; + P[3] = v[1]*v[1]; + P[4] = v[1]*v[2]; + P[5] = v[2]*v[2]; +} + +static inline void outerproduct3_scaled(const double* restrict v, double* restrict P, double scale) +{ + P[0] = scale * v[0]*v[0]; + P[1] = scale * v[0]*v[1]; + P[2] = scale * v[0]*v[2]; + P[3] = scale * v[1]*v[1]; + P[4] = scale * v[1]*v[2]; + P[5] = scale * v[2]*v[2]; +} + +// conjugate 2 vectors (a, b) through a symmetric matrix S: a->transpose x S x b +// (%i2) sym3 : matrix([s0,s1,s2], +// [s1,s3,s4], +// [s2,s4,s5]); + +// (%o2) matrix([s0,s1,s2],[s1,s3,s4],[s2,s4,s5]) +// (%i6) a : matrix([a0],[a1],[a2]); + +// (%o6) matrix([a0],[a1],[a2]) +// (%i7) b : matrix([b0],[b1],[b2]); + +// (%o7) matrix([b0],[b1],[b2]) +// (%i10) transpose(a) . sym3 . b; + +// (%o10) a2*(b2*s5+b1*s4+b0*s2)+a1*(b2*s4+b1*s3+b0*s1)+a0*(b2*s2+b1*s1+b0*s0) +static inline double conj_3(const double* restrict a, const double* restrict s, const double* restrict b) +{ + return a[2]*(b[2]*s[5]+b[1]*s[4]+b[0]*s[2])+a[1]*(b[2]*s[4]+b[1]*s[3]+b[0]*s[1])+a[0]*(b[2]*s[2]+b[1]*s[1]+b[0]*s[0]); +} + +// Given an orthonormal matrix, returns the det. This is always +1 or -1 +static inline double det_orthonormal33(const double* m) +{ + // cross(row0,row1) = det * row3 + + // I find a nice non-zero element of row3, and see if the signs match + if( m[6] < -0.1 ) + { + // looking at col0 of the last row. It is <0 + double cross = m[1]*m[5] - m[2]*m[4]; + return cross > 0.0 ? -1.0 : 1.0; + } + if( m[6] > 0.1) + { + // looking at col0 of the last row. It is > 0 + double cross = m[1]*m[5] - m[2]*m[4]; + return cross > 0.0 ? 1.0 : -1.0; + } + + if( m[7] < -0.1 ) + { + // looking at col1 of the last row. It is <0 + double cross = m[2]*m[3] - m[0]*m[5]; + return cross > 0.0 ? -1.0 : 1.0; + } + if( m[7] > 0.1) + { + // looking at col1 of the last row. It is > 0 + double cross = m[2]*m[3] - m[0]*m[5]; + return cross > 0.0 ? 1.0 : -1.0; + } + + if( m[8] < -0.1 ) + { + // looking at col2 of the last row. It is <0 + double cross = m[0]*m[4] - m[1]*m[3]; + return cross > 0.0 ? -1.0 : 1.0; + } + + // last option. This MUST be true, so I don't even bother to check + { + // looking at col2 of the last row. It is > 0 + double cross = m[0]*m[4] - m[1]*m[3]; + return cross > 0.0 ? 1.0 : -1.0; + } +} + +static void minimath_xchg(double* m, int i, int j) +{ + double t = m[i]; + m[i] = m[j]; + m[j] = t; +} +static inline void gen33_transpose(double* m) +{ + minimath_xchg(m, 1, 3); + minimath_xchg(m, 2, 6); + minimath_xchg(m, 5, 7); +} + +static inline void gen33_transpose_vout(const double* m, double* mout) +{ + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + mout[i*3+j] = m[j*3+i]; +} + +static inline double cofactors_gen33(// output + double* restrict c, + + // input + const double* restrict m) +{ + /* +(%i1) display2d : false; + +(%o1) false +(%i5) linel : 100000; + +(%o5) 100000 +(%i6) mat33 : matrix( [m0,m1,m2], [m3,m4,m5], [m6,m7,m8] ); + +(%o6) matrix([m0,m1,m2],[m3,m4,m5],[m6,m7,m8]) +(%i7) num( ev(invert(mat33)), detout ); + +(%o7) matrix([(m4*m8-m5*m7)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m2*m7-m1*m8)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m1*m5-m2*m4)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))],[(m5*m6-m3*m8)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m0*m8-m2*m6)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m2*m3-m0*m5)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))],[(m3*m7-m4*m6)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m1*m6-m0*m7)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m0*m4-m1*m3)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))]) +(%i8) determinant(mat33); + +(%o8) m0*(m4*m8-m5*m7)-m1*(m3*m8-m5*m6)+m2*(m3*m7-m4*m6) + */ + + double det = m[0]*(m[4]*m[8]-m[5]*m[7])-m[1]*(m[3]*m[8]-m[5]*m[6])+m[2]*(m[3]*m[7]-m[4]*m[6]); + + c[0] = m[4]*m[8]-m[5]*m[7]; + c[1] = m[2]*m[7]-m[1]*m[8]; + c[2] = m[1]*m[5]-m[2]*m[4]; + c[3] = m[5]*m[6]-m[3]*m[8]; + c[4] = m[0]*m[8]-m[2]*m[6]; + c[5] = m[2]*m[3]-m[0]*m[5]; + c[6] = m[3]*m[7]-m[4]*m[6]; + c[7] = m[1]*m[6]-m[0]*m[7]; + c[8] = m[0]*m[4]-m[1]*m[3]; + + return det; +} + +#ifdef __cplusplus +#undef restrict +#endif diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h new file mode 100644 index 00000000000..086810d4c69 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h @@ -0,0 +1,124 @@ +// 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 + +#pragma once + +// mrcal images. These are completely uninteresting, and don't do anything +// better that other image read/write APIS. If you have image libraries running, +// use those. If not, the ones defined here should be light and painless + +// I support several image types: +// - "uint8": 8-bit grayscale +// - "uint16": 16-bit grayscale (using the system endian-ness) +// - "bgr": 24-bit BGR color +// +// Each type defines several functions in the MRCAL_IMAGE_DECLARE() macro: +// +// - mrcal_image_TYPE_t container image +// - mrcal_image_TYPE_at(mrcal_image_TYPE_t* image, int x, int y) +// - mrcal_image_TYPE_at_const(const mrcal_image_TYPE_t* image, int x, int y) +// - mrcal_image_TYPE_t mrcal_image_TYPE_crop(mrcal_image_TYPE_t* image, in x0, int y0, int w, int h) +// - mrcal_image_TYPE_save (const char* filename, const mrcal_image_TYPE_t* image); +// - mrcal_image_TYPE_load( mrcal_image_TYPE_t* image, const char* filename); +// +// The image-loading functions require a few notes: +// +// An image structure to fill in is given. image->data will be allocated to the +// proper size. It is the caller's responsibility to free(image->data) when +// they're done. Usage sample: +// +// mrcal_image_uint8_t image; +// mrcal_image_uint8_load(&image, image_filename); +// .... do stuff ... +// free(image.data); +// +// mrcal_image_uint8_load() converts images to 8-bpp grayscale. Color and +// palettized images are accepted +// +// mrcal_image_uint16_load() does NOT convert images. The images being read must +// already be stored as 16bpp grayscale images +// +// mrcal_image_bgr_load() converts images to 24-bpp color + +#include +#include + + +typedef struct { uint8_t bgr[3]; } mrcal_bgr_t; + +#define MRCAL_IMAGE_DECLARE(T, Tname) \ +typedef struct \ +{ \ + union \ + { \ + /* in pixels */ \ + struct {int w, h;}; \ + struct {int width, height;}; \ + struct {int cols, rows;}; \ + }; \ + int stride; /* in bytes */ \ + T* data; \ +} mrcal_image_ ## Tname ## _t; \ + \ +static inline \ +T* mrcal_image_ ## Tname ## _at(mrcal_image_ ## Tname ## _t* image, int x, int y) \ +{ \ + return &image->data[x + y*image->stride / sizeof(T)]; \ +} \ + \ +static inline \ +const T* mrcal_image_ ## Tname ## _at_const(const mrcal_image_ ## Tname ## _t* image, int x, int y) \ +{ \ + return &image->data[x + y*image->stride / sizeof(T)]; \ +} \ + \ +static inline \ +mrcal_image_ ## Tname ## _t \ +mrcal_image_ ## Tname ## _crop(mrcal_image_ ## Tname ## _t* image, \ + int x0, int y0, \ + int w, int h) \ +{ \ + return (mrcal_image_ ## Tname ## _t){ .w = w, \ + .h = h, \ + .stride = image->stride, \ + .data = mrcal_image_ ## Tname ## _at(image,x0,y0) }; \ +} + +#define MRCAL_IMAGE_SAVE_LOAD_DECLARE(T, Tname) \ +bool mrcal_image_ ## Tname ## _save (const char* filename, const mrcal_image_ ## Tname ## _t* image); \ +bool mrcal_image_ ## Tname ## _load( mrcal_image_ ## Tname ## _t* image, const char* filename); + + +// Common images types +MRCAL_IMAGE_DECLARE(uint8_t, uint8); +MRCAL_IMAGE_DECLARE(uint16_t, uint16); +MRCAL_IMAGE_DECLARE(mrcal_bgr_t, bgr); +MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint8_t, uint8); +MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint16_t, uint16); +MRCAL_IMAGE_SAVE_LOAD_DECLARE(mrcal_bgr_t, bgr); + +// Uncommon types. Not everything supports these +MRCAL_IMAGE_DECLARE(int8_t, int8); +MRCAL_IMAGE_DECLARE(int16_t, int16); + +MRCAL_IMAGE_DECLARE(int32_t, int32); +MRCAL_IMAGE_DECLARE(uint32_t, uint32); +MRCAL_IMAGE_DECLARE(int64_t, int64); +MRCAL_IMAGE_DECLARE(uint64_t, uint64); + +MRCAL_IMAGE_DECLARE(float, float); +MRCAL_IMAGE_DECLARE(double, double); + +// Load the image into whatever type is stored on disk +bool mrcal_image_anytype_load(// output + // This is ONE of the known types + mrcal_image_uint8_t* image, + int* bits_per_pixel, + int* channels, + // input + const char* filename); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h new file mode 100644 index 00000000000..8fc2857758e --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h @@ -0,0 +1,109 @@ +// 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 + +#pragma once + +// THESE ARE NOT A PART OF THE EXTERNAL API. Exported for the mrcal python +// wrapper only + +// These models have no precomputed data +typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__precomputed_t; +typedef struct { int dummy; } mrcal_LENSMODEL_CAHVORE__precomputed_t; + +// The splined stereographic models configuration parameters can be used to +// compute the segment size. I cache this computation +typedef struct +{ + // The distance between adjacent knots (1 segment) is u_per_segment = + // 1/segments_per_u + double segments_per_u; +} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t; + +typedef struct +{ + bool ready; + union + { +#define PRECOMPUTED_STRUCT(s,n) mrcal_ ##s##__precomputed_t s##__precomputed; + MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT) +#undef PRECOMPUTED_STRUCT + }; +} mrcal_projection_precomputed_t; + + +void _mrcal_project_internal_opencv( // outputs + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, // may be NULL + double* dq_dintrinsics_nocore, // may be NULL + + // inputs + const mrcal_point3_t* p, + int N, + const double* intrinsics, + int Nintrinsics); +bool _mrcal_project_internal( // out + mrcal_point2_t* q, + + // Stored as a row-first array of shape (N,2,3). Each + // trailing ,3 dimension element is a mrcal_point3_t + mrcal_point3_t* dq_dp, + // core, distortions concatenated. Stored as a row-first + // array of shape (N,2,Nintrinsics). This is a DENSE array. + // High-parameter-count lens models have very sparse + // gradients here, and the internal project() function + // returns those sparsely. For now THIS function densifies + // all of these + double* dq_dintrinsics, + + // in + const mrcal_point3_t* p, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics, + + int Nintrinsics, + const mrcal_projection_precomputed_t* precomputed); +void _mrcal_precompute_lensmodel_data(mrcal_projection_precomputed_t* precomputed, + const mrcal_lensmodel_t* lensmodel); +bool _mrcal_unproject_internal( // out + mrcal_point3_t* out, + + // in + const mrcal_point2_t* q, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics, + const mrcal_projection_precomputed_t* precomputed); + +// Report the number of non-zero entries in the optimization jacobian +int _mrcal_num_j_nonzero(int Nobservations_board, + int Nobservations_point, + + // 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, + int Nframes, + int Npoints, int Npoints_fixed, + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h new file mode 100644 index 00000000000..df987b6486d --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h @@ -0,0 +1,396 @@ +// 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 + +#pragma once + +#include +#include + +#include "basic-geometry.h" + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Lens models +//////////////////////////////////////////////////////////////////////////////// + +// These are an "X macro": https://en.wikipedia.org/wiki/X_Macro +// +// The supported lens models and their parameter counts. Models with a +// configuration may have a dynamic parameter count; this is indicated here with +// a <0 value. These models report their parameter counts in the +// LENSMODEL_XXX__lensmodel_num_params() function, called by +// mrcal_lensmodel_num_params(). +#define MRCAL_LENSMODEL_NOCONFIG_LIST(_) \ + _(LENSMODEL_PINHOLE, 4) \ + _(LENSMODEL_STEREOGRAPHIC, 4) /* Simple stereographic-only model */ \ + _(LENSMODEL_LONLAT, 4) \ + _(LENSMODEL_LATLON, 4) \ + _(LENSMODEL_OPENCV4, 8) \ + _(LENSMODEL_OPENCV5, 9) \ + _(LENSMODEL_OPENCV8, 12) \ + _(LENSMODEL_OPENCV12, 16) /* available in OpenCV >= 3.0.0) */ \ + _(LENSMODEL_CAHVOR, 9) +#define MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(_) \ + _(LENSMODEL_CAHVORE, 12) +#define MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(_) \ + _(LENSMODEL_SPLINED_STEREOGRAPHIC, -1) +#define MRCAL_LENSMODEL_LIST(_) \ + MRCAL_LENSMODEL_NOCONFIG_LIST(_) \ + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(_) \ + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(_) + + +// parametric models have no extra configuration +typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__config_t; +typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__config_t; + +#define _MRCAL_ITEM_DEFINE_ELEMENT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) type name bitfield; + +// Configuration for CAHVORE. These are given as an an +// "X macro": https://en.wikipedia.org/wiki/X_Macro +#define MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_, cookie) \ + _(linearity, double, "d", ".2f", "lf", , cookie) +typedef struct +{ + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, ) +} mrcal_LENSMODEL_CAHVORE__config_t; + +// Configuration for the splined stereographic models. These are given as an an +// "X macro": https://en.wikipedia.org/wiki/X_Macro +#define MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(_, cookie) \ + /* Maximum degree of each 1D polynomial. This is almost certainly 2 */ \ + /* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */ \ + _(order, uint16_t, "H", PRIu16,SCNu16, , cookie) \ + /* We have a Nx by Ny grid of control points */ \ + _(Nx, uint16_t, "H", PRIu16,SCNu16, , cookie) \ + _(Ny, uint16_t, "H", PRIu16,SCNu16, , cookie) \ + /* The horizontal field of view. Not including fov_y. It's proportional with */ \ + /* Ny and Nx */ \ + _(fov_x_deg, uint16_t, "H", PRIu16,SCNu16, , cookie) +typedef struct +{ + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, ) +} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t; + + +// An X-macro-generated enum mrcal_lensmodel_type_t. This has an element for +// each entry in MRCAL_LENSMODEL_LIST (with "MRCAL_" prepended). This lensmodel +// type selects the lens model, but does NOT provide the configuration. +// mrcal_lensmodel_t does that. +#define _LIST_WITH_COMMA(s,n) ,MRCAL_ ## s +typedef enum + { // Generic error. Rarely used in current mrcal + MRCAL_LENSMODEL_INVALID = -2, + + // Configuration parsing failed + MRCAL_LENSMODEL_INVALID_BADCONFIG = -1, + + // A configuration was required, but was missing entirely + MRCAL_LENSMODEL_INVALID_MISSINGCONFIG = -3, + + // The model type wasn't known + MRCAL_LENSMODEL_INVALID_TYPE = -4, + + // Dummy entry. Must be -1 so that successive values start at 0 + _MRCAL_LENSMODEL_DUMMY = -1 + + // The rest, starting with 0 + MRCAL_LENSMODEL_LIST( _LIST_WITH_COMMA ) } mrcal_lensmodel_type_t; +#undef _LIST_WITH_COMMA + + +// Defines a lens model: the type AND the configuration values +typedef struct +{ + // The type of lensmodel. This is an enum, selecting elements of + // MRCAL_LENSMODEL_LIST (with "MRCAL_" prepended) + mrcal_lensmodel_type_t type; + + // A union of all the possible configuration structures. We pick the + // structure type based on the value of "type + union + { +#define CONFIG_STRUCT(s,n) mrcal_ ##s##__config_t s##__config; + MRCAL_LENSMODEL_LIST(CONFIG_STRUCT) +#undef CONFIG_STRUCT + }; +} mrcal_lensmodel_t; + + +typedef union +{ + struct + { + double x2, y2; + }; + double values[2]; +} mrcal_calobject_warp_t; + +#define MRCAL_NSTATE_CALOBJECT_WARP ((int)((sizeof(mrcal_calobject_warp_t)/sizeof(double)))) + +//// ADD CHANGES TO THE DOCS IN lensmodels.org +//// +// An X-macro-generated mrcal_lensmodel_metadata_t. Each lens model type has +// some metadata that describes its inherent properties. These properties can be +// queried by calling mrcal_lensmodel_metadata() in C and +// mrcal.lensmodel_metadata_and_config() in Python. The available properties and +// their meaning are listed in MRCAL_LENSMODEL_META_LIST() +#define MRCAL_LENSMODEL_META_LIST(_, cookie) \ + /* If true, this model contains an "intrinsics core". This is described */ \ + /* in mrcal_intrinsics_core_t. If present, the 4 core parameters ALWAYS */ \ + /* appear at the start of a model's parameter vector */ \ + _(has_core, bool, "i",,, :1, cookie) \ + \ + /* Whether a model is able to project points behind the camera */ \ + /* (z<0 in the camera coordinate system). Models based on a pinhole */ \ + /* projection (pinhole, OpenCV, CAHVOR(E)) cannot do this. models based */ \ + /* on a stereographic projection (stereographic, splined stereographic) */ \ + /* can */ \ + _(can_project_behind_camera, bool, "i",,, :1, cookie) \ + \ + /* Whether gradients are available for this model. Currently only */ \ + /* CAHVORE does not have gradients */ \ + _(has_gradients, bool, "i",,, :1, cookie) \ + \ + /* Whether this is a noncentral model.Currently the only noncentral */ \ + /* model we have is CAHVORE. There will be more later. */ \ + _(noncentral, bool, "i",,, :1, cookie) + +typedef struct +{ + MRCAL_LENSMODEL_META_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, ) +} mrcal_lensmodel_metadata_t; + + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Optimization +//////////////////////////////////////////////////////////////////////////////// + +// Used to specify which camera is making an observation. The "intrinsics" index +// is used to identify a specific camera, while the "extrinsics" index is used +// to locate a camera in space. If I have a camera that is moving over time, the +// intrinsics index will remain the same, while the extrinsics index will change +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: there should be a pool of these, and I should be indexing that pool" +#endif +typedef struct +{ + // indexes the intrinsics array + int intrinsics; + // indexes the extrinsics array. -1 means "at coordinate system reference" + int extrinsics; +} mrcal_camera_index_t; + + +// An observation of a calibration board. Each "observation" is ONE camera +// observing a board +typedef struct +{ + // which camera is making this observation + mrcal_camera_index_t icam; + + // indexes the "frames" array to select the pose of the calibration object + // being observed + int iframe; +} mrcal_observation_board_t; + +// An observation of a discrete point. Each "observation" is ONE camera +// observing a single point in space +typedef struct +{ + // which camera is making this observation + mrcal_camera_index_t icam; + + // indexes the "points" array to select the position of the point being + // observed + int i_point; +} mrcal_observation_point_t; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: triangulated points into a pool. maybe allowing the intrinsics to move in the process" +#endif + +// An observation of a discrete point where the point itself is NOT a part of +// the optimization, but computed implicitly via triangulation. This structure +// is very similar to mrcal_observation_point_t, except instead of i_point +// identifying the point being observed we have +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: FINISH DOC" +#endif +typedef struct +{ + // which camera is making this observation + mrcal_camera_index_t icam; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: DOCUMENT. CAN THIS BIT FIELD BE PACKED NICELY?" +#endif + // Set if this is the last camera observing this point. Any set of N>=2 + // cameras can observe any point. All observations of a given point are + // stored consecutively, the last one being noted by this bit +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: do I really need this? I cannot look at the next observation to determine when this one is done?" +#endif + bool last_in_set : 1; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: this is temporary. Should be a weight in observations_point_pool like all the other observations" +#endif + bool outlier : 1; + + // Observed pixel coordinates. This works just like elements of + // observations_board_pool and observations_point_pool + mrcal_point3_t px; +} mrcal_observation_point_triangulated_t; + + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #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" +#endif + + +// 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 +// everything else. OR we can ask mrcal to lock down some part of the +// optimization problem, and to solve for the rest. If any variables are locked +// down, we use their initial values passed-in to mrcal_optimize() +typedef struct +{ + // If true, we solve for the intrinsics core. Applies only to those models + // that HAVE a core (fx,fy,cx,cy) + bool do_optimize_intrinsics_core : 1; + + // If true, solve for the non-core lens parameters + bool do_optimize_intrinsics_distortions : 1; + + // If true, solve for the geometry of the cameras + bool do_optimize_extrinsics : 1; + + // If true, solve for the poses of the calibration object + bool do_optimize_frames : 1; + + // If true, optimize the shape of the calibration object + bool do_optimize_calobject_warp : 1; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: Need finer-grained regularization flags" +// #warning "triangulated-solve: Regularization flags should reflect do_optimize stuff and Ncameras stuff" +#endif + // If true, apply the regularization terms in the solver + bool do_apply_regularization : 1; + + // Whether to try to find NEW outliers. The outliers given on + // input are respected regardless + bool do_apply_outlier_rejection : 1; + + // Pull the distance between the first two cameras to 1.0 + bool do_apply_regularization_unity_cam01: 1; + +} mrcal_problem_selections_t; + +// Constants used in a mrcal optimization. This is similar to +// mrcal_problem_selections_t, but contains numerical values rather than just +// bits +typedef struct +{ + // The minimum distance of an observed discrete point from its observing + // camera. Any observation of a point below this range will be penalized to + // encourage the optimizer to move the point further away from the camera + double point_min_range; + + + // The maximum distance of an observed discrete point from its observing + // camera. Any observation of a point abive this range will be penalized to + // encourage the optimizer to move the point closer to the camera + double point_max_range; +} mrcal_problem_constants_t; + + +// An X-macro-generated mrcal_stats_t. This structure is returned by the +// optimizer, and contains some statistics about the optimization +#define MRCAL_STATS_ITEM(_) \ + /* The RMS error of the optimized fit at the optimum. Generally the residual */ \ + /* vector x contains error values for each element of q, so N observed pixels */ \ + /* produce 2N measurements: len(x) = 2*N. And the RMS error is */ \ + /* sqrt( norm2(x) / N ) */ \ + _(double, rms_reproj_error__pixels, PyFloat_FromDouble) \ + \ + /* 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) +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: implement stats.Noutliers_triangulated_point; add to c-api.org" +#endif +#define MRCAL_STATS_ITEM_DEFINE(type, name, pyconverter) type name; +typedef struct +{ + MRCAL_STATS_ITEM(MRCAL_STATS_ITEM_DEFINE) +} mrcal_stats_t; + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Layout of the measurement and state vectors +//////////////////////////////////////////////////////////////////////////////// + +// The "intrinsics core" of a camera. This defines the final step of a +// projection operation. For instance with a pinhole model we have +// +// q[0] = focal_xy[0] * x/z + center_xy[0] +// q[1] = focal_xy[1] * y/z + center_xy[1] +typedef struct +{ + double focal_xy [2]; + double center_xy[2]; +} mrcal_intrinsics_core_t; + + +// structure containing a camera pose + lens model. Used for .cameramodel +// input/output +#define MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS \ + double rt_cam_ref[6]; \ + unsigned int imagersize[2]; \ + mrcal_lensmodel_t lensmodel \ + +typedef struct +{ + MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS; + // mrcal_cameramodel_t works for all lensmodels, so its intrinsics count is + // not known at compile time. mrcal_cameramodel_t is thus usable only as + // part of a larger structure or as a mrcal_cameramodel_t* argument to + // functions. To allocate new mrcal_cameramodel_t objects, use + // mrcal_cameramodel_LENSMODEL_XXX_t or malloc() with the proper intrinsics + // size taken into account + double intrinsics[0]; +} mrcal_cameramodel_t; + + +#define DEFINE_mrcal_cameramodel_MODEL_t(s,n) \ +typedef union \ +{ \ + mrcal_cameramodel_t m; \ + struct \ + { \ + MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS; \ + double intrinsics[n]; \ + }; \ +} mrcal_cameramodel_ ## s ## _t; + + +MRCAL_LENSMODEL_NOCONFIG_LIST( DEFINE_mrcal_cameramodel_MODEL_t) +MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(DEFINE_mrcal_cameramodel_MODEL_t) diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h new file mode 100644 index 00000000000..b61b4878353 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h @@ -0,0 +1,924 @@ +// 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 + +#pragma once + +#include +#include + +#include "mrcal-types.h" +#include "poseutils.h" +#include "stereo.h" +#include "triangulation.h" +#include "mrcal-image.h" + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Lens models +//////////////////////////////////////////////////////////////////////////////// +#ifdef __cplusplus +extern "C" { +#endif +// Return an array of strings listing all the available lens models +// +// These are all "unconfigured" strings that use "..." placeholders for any +// configuration values. Each returned string is a \0-terminated const char*. The +// end of the list is signified by a NULL pointer +const char* const* mrcal_supported_lensmodel_names( void ); // NULL-terminated array of char* strings + + +// Return true if the given mrcal_lensmodel_type_t specifies a valid lens model + +static bool mrcal_lensmodel_type_is_valid(mrcal_lensmodel_type_t t) +{ + return t >= 0; +} + +// Evaluates to true if the given lens model is one of the supported OpenCV +// types +#define MRCAL_LENSMODEL_IS_OPENCV(d) (MRCAL_LENSMODEL_OPENCV4 <= (d) && (d) <= MRCAL_LENSMODEL_OPENCV12) + + +// Return a string describing a lens model. +// +// This function returns a static string. For models with no configuration, this +// is the FULL string for that model. For models with a configuration, the +// configuration values have "..." placeholders. These placeholders mean that +// the resulting strings do not define a lens model fully, and cannot be +// converted to a mrcal_lensmodel_t with mrcal_lensmodel_from_name() +// +// This is the inverse of mrcal_lensmodel_type_from_name() +const char* mrcal_lensmodel_name_unconfigured( const mrcal_lensmodel_t* lensmodel ); + + +// Return a CONFIGURED string describing a lens model. +// +// This function generates a fully-configured string describing the given lens +// model. For models with no configuration, this is just the static string +// returned by mrcal_lensmodel_name_unconfigured(). For models that have a +// configuration, however, the configuration values are filled-in. The resulting +// string may be converted back into a mrcal_lensmodel_t by calling +// mrcal_lensmodel_from_name(). +// +// This function writes the string into the given buffer "out". The size of the +// buffer is passed in the "size" argument. The meaning of "size" is as with +// snprintf(), which is used internally. Returns true on success +// +// This is the inverse of mrcal_lensmodel_from_name() +bool mrcal_lensmodel_name( char* out, int size, + const mrcal_lensmodel_t* lensmodel ); + + +// Parse the lens model type from a lens model name string +// +// The configuration is ignored. Thus this function works even if the +// configuration is missing or unparseable. Unknown model names return +// MRCAL_LENSMODEL_INVALID_TYPE +// +// This is the inverse of mrcal_lensmodel_name_unconfigured() +mrcal_lensmodel_type_t mrcal_lensmodel_type_from_name( const char* name ); + + +// Parse the full configured lens model from a lens model name string +// +// The lens mode type AND the configuration are read into a mrcal_lensmodel_t +// structure, which this function returns. +// +// On error returns false with lensmodel->type set to MRCAL_LENSMODEL_INVALID_... +// +// This is the inverse of mrcal_lensmodel_name() +bool mrcal_lensmodel_from_name( // output + mrcal_lensmodel_t* lensmodel, + + // input + const char* name ); + +// Return a structure containing a model's metadata +// +// The available metadata is described in the definition of the +// MRCAL_LENSMODEL_META_LIST() macro +mrcal_lensmodel_metadata_t mrcal_lensmodel_metadata( const mrcal_lensmodel_t* lensmodel ); + + +// Return the number of parameters required to specify a given lens model +// +// For models that have a configuration, the parameter count value generally +// depends on the configuration. For instance, splined models use the model +// parameters as the spline control points, so the spline density (specified in +// the configuration) directly affects how many parameters such a model requires +int mrcal_lensmodel_num_params( const mrcal_lensmodel_t* lensmodel ); + + +// Return the locations of x and y spline knots + +// Splined models are defined by the locations of their control points. These +// are arranged in a grid, the size and density of which is set by the model +// configuration. We fill-in the x knot locations into ux[] and the y locations +// into uy[]. ux[] and uy[] must be large-enough to hold configuration->Nx and +// configuration->Ny values respectively. +// +// This function applies to splined models only. Returns true on success +bool mrcal_knots_for_splined_models( double* ux, double* uy, + const mrcal_lensmodel_t* lensmodel); + + + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Projections +//////////////////////////////////////////////////////////////////////////////// + +// Project the given camera-coordinate-system points +// +// Compute a "projection", a mapping of points defined in the camera coordinate +// system to their observed pixel coordinates. If requested, gradients are +// computed as well. +// +// We project N 3D points p to N 2D pixel coordinates q using the given +// lensmodel and intrinsics parameter values. +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +// +// if (dq_dintrinsics != NULL) we report the gradient dq/dintrinsics in a dense +// (N,2,Nintrinsics) array. Note that splined models have very high Nintrinsics +// and very sparse gradients. THIS function reports the gradients densely, +// however, so it is inefficient for splined models. +// +// This function supports CAHVORE distortions only if we don't ask for any +// gradients +// +// Projecting out-of-bounds points (beyond the field of view) returns undefined +// values. Generally things remain continuous even as we move off the imager +// domain. Pinhole-like projections will work normally if projecting a point +// behind the camera. Splined projections clamp to the nearest spline segment: +// the projection will fly off to infinity quickly since we're extrapolating a +// polynomial, but the function will remain continuous. +bool mrcal_project( // out + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, + double* dq_dintrinsics, + + // in + const mrcal_point3_t* p, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics); + + +// Unproject the given pixel coordinates +// +// Compute an "unprojection", a mapping of pixel coordinates to the camera +// coordinate system. +// +// We unproject N 2D pixel coordinates q to N 3D direction vectors v using the +// given lensmodel and intrinsics parameter values. The returned vectors v are +// not normalized, and may have any length. + +// This is the "reverse" direction, so an iterative nonlinear optimization is +// performed internally to compute this result. This is much slower than +// mrcal_project(). For OpenCV models specifically, OpenCV has +// cvUndistortPoints() (and cv2.undistortPoints()), but these are unreliable: +// https://github.com/opencv/opencv/issues/8811 +// +// This function does NOT support CAHVORE +bool mrcal_unproject( // out + mrcal_point3_t* v, + + // in + const mrcal_point2_t* q, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics); + + +// 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 +// +// This is a simplified special case of mrcal_project(). We project N +// camera-coordinate-system points p to N pixel coordinates q +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +void mrcal_project_pinhole( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, + + // input + const mrcal_point3_t* p, + int N, + const double* fxycxy); + + +// Unproject the given pixel coordinates using a pinhole model. +// See the docs for projection details: +// https://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 +// vectors v are not normalized, and may have any length. +// +// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array +// ((N,3) mrcal_point2_t objects). +void mrcal_unproject_pinhole( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy); + +// 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 +// +// This is a simplified special case of mrcal_project(). We project N +// camera-coordinate-system points p to N pixel coordinates q +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +void mrcal_project_stereographic( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dp, + + // input + const mrcal_point3_t* p, + int N, + const double* fxycxy); + + +// Unproject the given pixel coordinates using a stereographic model. +// See the docs for projection details: +// https://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 +// vectors v are not normalized, and may have any length. +// +// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array +// ((N,3) mrcal_point2_t objects). +void mrcal_unproject_stereographic( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy); + + +// 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 +// +// This is a simplified special case of mrcal_project(). We project N +// camera-coordinate-system points p to N pixel coordinates q +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +void mrcal_project_lonlat( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dv, // May be NULL. Each point + // gets a block of 2 mrcal_point3_t + // objects + + // input + const mrcal_point3_t* v, + int N, + const double* fxycxy); + +// Unproject the given pixel coordinates using an equirectangular projection. +// See the docs for projection details: +// https://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 +// vectors v are normalized. +// +// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array +// ((N,3) mrcal_point2_t objects). +void mrcal_unproject_lonlat( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, // May be NULL. Each point + // gets a block of 3 mrcal_point2_t + // objects + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy); + + +// 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 +// +// This is a simplified special case of mrcal_project(). We project N +// camera-coordinate-system points p to N pixel coordinates q +// +// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array +// ((N,2) mrcal_point3_t objects). +void mrcal_project_latlon( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dv, // May be NULL. Each point + // gets a block of 2 mrcal_point3_t + // objects + + // input + const mrcal_point3_t* v, + int N, + const double* fxycxy); + +// Unproject the given pixel coordinates using a transverse equirectangular +// projection. See the docs for projection details: +// https://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 +// vectors v are normalized. +// +// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array +// ((N,3) mrcal_point2_t objects). +void mrcal_unproject_latlon( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, // May be NULL. Each point + // gets a block of 3 mrcal_point2_t + // objects + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy); + + + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// 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 +// problem selections. Any intrinsics parameters locked down in the +// mrcal_problem_selections_t do NOT count towards the optimization parameters +int mrcal_num_intrinsics_optimization_params( mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel ); + + +// Scales a state vector to the packed, unitless form used by the optimizer +// +// In order to make the optimization well-behaved, we scale all the variables in +// the state and the gradients before passing them to the optimizer. The internal +// optimization library thus works only with unitless (or "packed") data. +// +// This function takes an (Nstate,) array of full-units values b[], and scales +// it to produce packed data. This function applies the scaling directly to the +// input array; the input is modified, and nothing is returned. +// +// This is the inverse of mrcal_unpack_solver_state_vector() +void mrcal_pack_solver_state_vector( // out, in + double* b, + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); + + +// Scales a state vector from the packed, unitless form used by the optimizer +// +// In order to make the optimization well-behaved, we scale all the variables in +// the state and the gradients before passing them to the optimizer. The internal +// optimization library thus works only with unitless (or "packed") data. +// +// This function takes an (Nstate,) array of unitless values b[], and scales it +// to produce full-units data. This function applies the scaling directly to the +// input array; the input is modified, and nothing is returned. +// +// This is the inverse of mrcal_pack_solver_state_vector() +void mrcal_unpack_solver_state_vector( // out, in + double* b, // unitless state on input, + // scaled, meaningful state on + // output + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); + + +// Reports the icam_extrinsics corresponding to a given icam_intrinsics. +// +// If we're solving a vanilla calibration problem (stationary cameras observing +// a moving calibration object), each camera has a unique intrinsics index and a +// unique extrinsics index. This function reports the latter, given the former. +// +// On success, the result is written to *icam_extrinsics, and we return true. If +// the given camera is at the reference coordinate system, it has no extrinsics, +// and we report -1. +// +// If we have moving cameras (NOT a vanilla calibration problem), there isn't a +// single icam_extrinsics for a given icam_intrinsics, and we report an error by +// returning false +bool mrcal_corresponding_icam_extrinsics(// out + int* icam_extrinsics, + + // in + int icam_intrinsics, + int Ncameras_intrinsics, + int Ncameras_extrinsics, + int Nobservations_board, + const mrcal_observation_board_t* observations_board, + int Nobservations_point, + const mrcal_observation_point_t* observations_point); + +// Solve the given optimization problem +// +// This is the entry point to the mrcal optimization routine. The argument list +// is commented. +mrcal_stats_t +mrcal_optimize( // out + // Each one of these output pointers may be NULL + // Shape (Nstate,) + 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, + + // Shape (Nmeasurements,) + double* x, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_x, + + // out, in + + // These are a seed on input, solution on output + + // intrinsics is a concatenation of the intrinsics core and the + // distortion params. The specific distortion parameters may + // vary, depending on lensmodel, so this is a variable-length + // structure + double* intrinsics, // Ncameras_intrinsics * NlensParams + mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame + mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame + mrcal_point3_t* points, // Npoints of these. In the reference frame + mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + int Nobservations_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. + // + // .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, + const mrcal_problem_constants_t* problem_constants, + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n, + bool verbose, + + 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 +struct cholmod_sparse_struct; +struct cholmod_factor_struct; +struct cholmod_common_struct; + +// Evaluate the value of the callback function at the given operating point +// +// The main optimization routine in mrcal_optimize() searches for optimal +// parameters by repeatedly calling a function to evaluate each hypothethical +// parameter set. This evaluation function is available by itself here, +// separated from the optimization loop. The arguments are largely the same as +// those to mrcal_optimize(), but the inputs are all read-only It is expected +// that this will be called from Python only. +bool mrcal_optimizer_callback(// out + + // These output pointers may NOT be NULL, unlike + // their analogues in mrcal_optimize() + + // Shape (Nstate,) + 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, + + // Shape (Nmeasurements,) + double* x, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_x, + + // output Jacobian. May be NULL if we don't need + // it. This is the unitless Jacobian, used by the + // internal optimization routines + struct cholmod_sparse_struct* Jt, + + + // in + + // intrinsics is a concatenation of the intrinsics core + // and the distortion params. The specific distortion + // parameters may vary, depending on lensmodel, so + // this is a variable-length structure + const double* intrinsics, // Ncameras_intrinsics * NlensParams + const mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame + const mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame + const mrcal_point3_t* points, // Npoints of these. In the reference frame + const mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + 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. + // + // .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, + const mrcal_problem_constants_t* problem_constants, + double calibration_object_spacing, + int calibration_object_width_n, + 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 +//////////////////////////////////////////////////////////////////////////////// + +// The optimization routine tries to minimize the length of the measurement +// vector x by moving around the state vector b. +// +// Depending on the specific optimization problem being solved and the +// mrcal_problem_selections_t, the state vector may contain any of +// - The lens parameters +// - The geometry of the cameras +// - The geometry of the observed chessboards and discrete points +// - The chessboard shape +// +// The measurement vector may contain +// - The errors in observations of the chessboards +// - The errors in observations of discrete points +// - The penalties in the solved point positions +// - The regularization terms +// +// Given the problem selections and a vector b or x it is often useful to know +// where specific quantities lie in those vectors. We have 4 sets of functions +// to answer such questions: +// +// int mrcal_measurement_index_THING() +// Returns the index in the measurement vector x where the contiguous block of +// values describing the THING begins. THING is any of +// - boards +// - points +// - regularization +// +// int mrcal_num_measurements_THING() +// Returns the number of values in the contiguous block in the measurement +// vector x that describe the given THING. THING is any of +// - boards +// - points +// - regularization +// +// int mrcal_state_index_THING() +// Returns the index in the state vector b where the contiguous block of +// values describing the THING begins. THING is any of +// - intrinsics +// - extrinsics +// - frames +// - points +// - calobject_warp +// If we're not optimizing the THING, return <0 +// +// int mrcal_num_states_THING() +// Returns the number of values in the contiguous block in the state +// vector b that describe the given THING. THING is any of +// - intrinsics +// - extrinsics +// - frames +// - points +// - calobject_warp +// If we're not optimizing the THING, return 0 +int mrcal_measurement_index_boards(int i_observation_board, + int Nobservations_board, + int Nobservations_point, + int calibration_object_width_n, + int calibration_object_height_n); +int mrcal_num_measurements_boards(int Nobservations_board, + int calibration_object_width_n, + int calibration_object_height_n); +int mrcal_measurement_index_points(int i_observation_point, + int Nobservations_board, + int Nobservations_point, + int calibration_object_width_n, + int calibration_object_height_n); +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 calibration_object_height_n, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, int Nobservations_point, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* 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, + int Nframes, + int Npoints, int Npoints_fixed, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); + +int mrcal_num_states(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_state_index_intrinsics(int icam_intrinsics, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_intrinsics(int Ncameras_intrinsics, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_state_index_extrinsics(int icam_extrinsics, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_extrinsics(int Ncameras_extrinsics, + mrcal_problem_selections_t problem_selections); +int mrcal_state_index_frames(int iframe, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_frames(int Nframes, + mrcal_problem_selections_t problem_selections); +int mrcal_state_index_points(int i_point, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_points(int Npoints, int Npoints_fixed, + mrcal_problem_selections_t problem_selections); +int mrcal_state_index_calobject_warp(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel); +int mrcal_num_states_calobject_warp(mrcal_problem_selections_t problem_selections, + int Nobservations_board); + + +// if len>0, the string doesn't need to be 0-terminated. If len<=0, the end of +// the buffer IS indicated by a '\0' byte +// +// return NULL on error +mrcal_cameramodel_t* mrcal_read_cameramodel_string(const char* string, int len); +mrcal_cameramodel_t* mrcal_read_cameramodel_file (const char* filename); +void mrcal_free_cameramodel(mrcal_cameramodel_t** cameramodel); + +bool mrcal_write_cameramodel_file(const char* filename, + const mrcal_cameramodel_t* cameramodel); + +#ifdef __cplusplus +} // extern "C" +#endif +#define DECLARE_mrcal_apply_color_map(T,Tname) \ + bool mrcal_apply_color_map_##Tname( \ + mrcal_image_bgr_t* out, \ + const mrcal_image_##Tname##_t* in, \ + \ + /* If true, I set in_min/in_max from the */ \ + /* min/max of the input data */ \ + const bool auto_min, \ + const bool auto_max, \ + \ + /* If true, I implement gnuplot's default 7,5,15 mapping. */ \ + /* This is a reasonable default choice. */ \ + /* function_red/green/blue are ignored if true */ \ + const bool auto_function, \ + \ + /* min/max input values to use if not */ \ + /* auto_min/auto_max */ \ + T in_min, /* will map to 0 */ \ + T in_max, /* will map to 255 */ \ + \ + /* The color mappings to use. If !auto_function */ \ + int function_red, \ + int function_green, \ + int function_blue) + +DECLARE_mrcal_apply_color_map(uint8_t, uint8); +DECLARE_mrcal_apply_color_map(uint16_t, uint16); +DECLARE_mrcal_apply_color_map(uint32_t, uint32); +DECLARE_mrcal_apply_color_map(uint64_t, uint64); + +DECLARE_mrcal_apply_color_map(int8_t, int8); +DECLARE_mrcal_apply_color_map(int16_t, int16); +DECLARE_mrcal_apply_color_map(int32_t, int32); +DECLARE_mrcal_apply_color_map(int64_t, int64); + +DECLARE_mrcal_apply_color_map(float, float); +DECLARE_mrcal_apply_color_map(double, double); + +#undef DECLARE_mrcal_apply_color_map + + + +// returns false on error +typedef bool (*mrcal_callback_sensor_link_t)(const uint16_t idx_to, + const uint16_t idx_from, + void* cookie); + +// Traverses a connectivity graph of sensors to find the best connection from +// the root sensor (idx==0) to every other sensor. This is useful to seed a +// problem with sparse connections, where every sensor doesn't have overlapping +// observations with every other sensor. See the docstring for +// mrcal.traverse_sensor_links() for details (that Python function wraps +// this one). Note: this C function takes a packed connectivity matrix (just the +// upper triangle stored), while the Python function takes a full (N,N) array, +// while assuming it is symmetric and has a 0 diagonal +// +// returns false on error +bool mrcal_traverse_sensor_links( const uint16_t Nsensors, + + // (N,N) symmetric matrix with a 0 diagonal. + // I store the upper triangle only, + // row-first: a 1D array of (N*(N-1)/2) + // values. use pairwise_index() to index + const uint16_t* connectivity_matrix, + const mrcal_callback_sensor_link_t cb, + void* cookie); + + +// Public ABI stuff, that's not for end-user consumption +#include "mrcal-internal.h" + diff --git a/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h b/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h new file mode 100644 index 00000000000..23b1c772cfd --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/poseutils.h @@ -0,0 +1,586 @@ +// 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 + +#pragma once + +#include + +// Unless specified all arrays stored in contiguous matrices in row-major order. +// +// All functions are defined using the mrcal_..._full() form, which supports +// non-contiguous input and output arrays, and some optional arguments. Strides +// are used to specify the array layout. +// +// All functions have a convenience wrapper macro that is a simpler way to call +// the function, usable with contiguous arrays and defaults. +// +// All the functions use double-precision floating point to store the data, and +// C ints to store strides. The strides are given in bytes. In the +// mrcal_..._full() functions, each array is followed by the strides, one per +// dimension. +// +// I have two different representations of pose transformations: +// +// - Rt is a concatenated (4,3) array: Rt = nps.glue(R,t, axis=-2). The +// transformation is R*x+t +// +// - rt is a concatenated (6,) array: rt = nps.glue(r,t, axis=-1). The +// transformation is R*x+t where R = R_from_r(r) +// +// I treat all vectors as column vectors, so matrix multiplication works from +// the left: to rotate a vector x by a rotation matrix R I have +// +// x_rotated = R * x + + +// Store an identity rotation matrix into the given (3,3) array +// +// This is simply an identity matrix +#define mrcal_identity_R(R) mrcal_identity_R_full(R,0,0) +void mrcal_identity_R_full(double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Store an identity rodrigues rotation into the given (3,) array +// +// This is simply an array of zeros +#define mrcal_identity_r(r) mrcal_identity_r_full(r,0) +void mrcal_identity_r_full(double* r, // (3,) array + int r_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Store an identity Rt transformation into the given (4,3) array +#define mrcal_identity_Rt(Rt) mrcal_identity_Rt_full(Rt,0,0) +void mrcal_identity_Rt_full(double* Rt, // (4,3) array + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Store an identity rt transformation into the given (6,) array +#define mrcal_identity_rt(rt) mrcal_identity_rt_full(rt,0) +void mrcal_identity_rt_full(double* rt, // (6,) array + int rt_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Rotate the point x_in in a (3,) array by the rotation matrix R in a (3,3) +// array. This is simply the matrix-vector multiplication R x_in +// +// The result is returned in a (3,) array x_out. +// +// The gradient dx_out/dR is returned in a (3, 3,3) array J_R. Set to NULL if +// this is not wanted +// +// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply +// the matrix R. Set to NULL if this is not wanted +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_rotate_point_R( x_out,J_R,J_x,R,x_in) mrcal_rotate_point_R_full(x_out,0,J_R,0,0,0,J_x,0,0,R,0,0,x_in,0, false) +#define mrcal_rotate_point_R_inverted(x_out,J_R,J_x,R,x_in) mrcal_rotate_point_R_full(x_out,0,J_R,0,0,0,J_x,0,0,R,0,0,x_in,0, true) +void mrcal_rotate_point_R_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_R, // (3,3,3) array. May be NULL + int J_R_stride0, // in bytes. <= 0 means "contiguous" + int J_R_stride1, // in bytes. <= 0 means "contiguous" + int J_R_stride2, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* R, // (3,3) array. May be NULL + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // rotation in the opposite + // direction. J_R corresponds + // to the input R + ); + +// Rotate the point x_in in a (3,) array by the rodrigues rotation in a (3,) +// array. +// +// The result is returned in a (3,) array x_out. +// +// The gradient dx_out/dr is returned in a (3,3) array J_r. Set to NULL if this +// is not wanted +// +// The gradient dx_out/dx_in is returned in a (3,3) array J_x. Set to NULL if +// this is not wanted +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_rotate_point_r( x_out,J_r,J_x,r,x_in) mrcal_rotate_point_r_full(x_out,0,J_r,0,0,J_x,0,0,r,0,x_in,0, false) +#define mrcal_rotate_point_r_inverted(x_out,J_r,J_x,r,x_in) mrcal_rotate_point_r_full(x_out,0,J_r,0,0,J_x,0,0,r,0,x_in,0, true) +void mrcal_rotate_point_r_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_r, // (3,3) array. May be NULL + int J_r_stride0, // in bytes. <= 0 means "contiguous" + int J_r_stride1, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r, // (3,) array. May be NULL + int r_stride0, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // rotation in the opposite + // direction. J_r corresponds + // to the input r + ); + + +// Transform the point x_in in a (3,) array by the Rt transformation in a (4,3) +// array. +// +// The result is returned in a (3,) array x_out. +// +// The gradient dx_out/dRt is returned in a (3, 4,3) array J_Rt. Set to NULL if +// this is not wanted +// +// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply +// the matrix R. Set to NULL if this is not wanted +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_transform_point_Rt( x_out,J_Rt,J_x,Rt,x_in) mrcal_transform_point_Rt_full(x_out,0,J_Rt,0,0,0,J_x,0,0,Rt,0,0,x_in,0, false) +#define mrcal_transform_point_Rt_inverted(x_out,J_Rt,J_x,Rt,x_in) mrcal_transform_point_Rt_full(x_out,0,J_Rt,0,0,0,J_x,0,0,Rt,0,0,x_in,0, true) +void mrcal_transform_point_Rt_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_Rt, // (3,4,3) array. May be NULL + int J_Rt_stride0, // in bytes. <= 0 means "contiguous" + int J_Rt_stride1, // in bytes. <= 0 means "contiguous" + int J_Rt_stride2, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt, // (4,3) array. May be NULL + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // transformation in the opposite + // direction. J_Rt corresponds + // to the input Rt + ); + +// Transform the point x_in in a (3,) array by the rt transformation in a (6,) +// array. +// +// The result is returned in a (3,) array x_out. +// +// The gradient dx_out/drt is returned in a (3,6) array J_rt. Set to NULL if +// this is not wanted +// +// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply +// the matrix R. Set to NULL if this is not wanted +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_transform_point_rt( x_out,J_rt,J_x,rt,x_in) mrcal_transform_point_rt_full(x_out,0,J_rt,0,0,J_x,0,0,rt,0,x_in,0, false) +#define mrcal_transform_point_rt_inverted(x_out,J_rt,J_x,rt,x_in) mrcal_transform_point_rt_full(x_out,0,J_rt,0,0,J_x,0,0,rt,0,x_in,0, true) +void mrcal_transform_point_rt_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_rt, // (3,6) array. May be NULL + int J_rt_stride0, // in bytes. <= 0 means "contiguous" + int J_rt_stride1, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt, // (6,) array. May be NULL + int rt_stride0, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply the + // transformation in the + // opposite direction. + // J_rt corresponds to + // the input rt + ); + +// Convert a rotation matrix in a (3,3) array to a rodrigues vector in a (3,) +// array +// +// The result is returned in a (3,) array r +// +// The gradient dr/dR is returned in a (3, 3,3) array J. Set to NULL if this is +// not wanted +#define mrcal_r_from_R(r,J,R) mrcal_r_from_R_full(r,0,J,0,0,0,R,0,0) +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" + ); + +// Convert a rodrigues vector in a (3,) array to a rotation matrix in a (3,3) +// array +// +// The result is returned in a (3,3) array R +// +// The gradient dR/dr is returned in a (3,3 ,3) array J. Set to NULL if this is +// not wanted +#define mrcal_R_from_r(R,J,r) mrcal_R_from_r_full(R,0,0,J,0,0,0,r,0) +void mrcal_R_from_r_full( // outputs + double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1, // 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,) vector + int r_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Invert a rotation matrix. This is a transpose +// +// The input is given in R_in in a (3,3) array +// +// The result is returned in a (3,3) array R_out +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_invert_R(R_out,R_in) mrcal_invert_R_full(R_out,0,0,R_in,0,0) +void mrcal_invert_R_full( // output + double* R_out, // (3,3) array + int R_out_stride0, // in bytes. <= 0 means "contiguous" + int R_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* R_in, // (3,3) array + int R_in_stride0, // in bytes. <= 0 means "contiguous" + int R_in_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Convert an Rt transformation in a (4,3) array to an rt transformation in a +// (6,) array +// +// The result is returned in a (6,) array rt +// +// The gradient dr/dR is returned in a (3, 3,3) array J_R. Set to NULL if this +// is not wanted +// +// The t terms are identical, so dt/dt = identity and I do not return it +// +// The r and R terms are independent of the t terms, so dr/dt and dt/dR are both +// 0, and I do not return them +#define mrcal_rt_from_Rt(rt,J_R,Rt) mrcal_rt_from_Rt_full(rt,0,J_R,0,0,0,Rt,0,0) +void mrcal_rt_from_Rt_full( // output + double* rt, // (6,) vector + int rt_stride0, // in bytes. <= 0 means "contiguous" + double* J_R, // (3,3,3) array. Gradient. May be NULL + // No J_t. It's always the identity + int J_R_stride0, // in bytes. <= 0 means "contiguous" + int J_R_stride1, // in bytes. <= 0 means "contiguous" + int J_R_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt, // (4,3) array + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Convert an rt transformation in a (6,) array to an Rt transformation in a +// (4,3) array +// +// The result is returned in a (4,3) array Rt +// +// The gradient dR/dr is returned in a (3,3 ,3) array J_r. Set to NULL if this +// is not wanted +// +// The t terms are identical, so dt/dt = identity and I do not return it +// +// The r and R terms are independent of the t terms, so dR/dt and dt/dr are both +// 0, and I do not return them +#define mrcal_Rt_from_rt(Rt,J_r,rt) mrcal_Rt_from_rt_full(Rt,0,0,J_r,0,0,0,rt,0) +void mrcal_Rt_from_rt_full( // output + double* Rt, // (4,3) array + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1, // in bytes. <= 0 means "contiguous" + double* J_r, // (3,3,3) array. Gradient. May be NULL + // No J_t. It's just the identity + int J_r_stride0, // in bytes. <= 0 means "contiguous" + int J_r_stride1, // in bytes. <= 0 means "contiguous" + int J_r_stride2, // in bytes. <= 0 means "contiguous" + + // input + const double* rt, // (6,) vector + int rt_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Invert an Rt transformation +// +// The input is given in Rt_in in a (4,3) array +// +// The result is returned in a (4,3) array Rt_out +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_invert_Rt(Rt_out,Rt_in) mrcal_invert_Rt_full(Rt_out,0,0,Rt_in,0,0) +void mrcal_invert_Rt_full( // output + double* Rt_out, // (4,3) array + int Rt_out_stride0, // in bytes. <= 0 means "contiguous" + int Rt_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt_in, // (4,3) array + int Rt_in_stride0, // in bytes. <= 0 means "contiguous" + int Rt_in_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Invert an rt transformation +// +// The input is given in rt_in in a (6,) array +// +// The result is returned in a (6,) array rt_out +// +// The gradient dtout/drin is returned in a (3,3) array dtout_drin. Set to NULL +// if this is not wanted +// +// The gradient dtout/dtin is returned in a (3,3) array dtout_dtin. Set to NULL +// if this is not wanted +// +// The gradient drout/drin is always -identity. So it is not returned +// +// The gradient drout/dtin is always 0. So it is not returned +// +// In-place operation is supported; the output array may be the same as the +// input arrays to overwrite the input. +#define mrcal_invert_rt(rt_out,dtout_drin,dtout_dtin,rt_in) mrcal_invert_rt_full(rt_out,0,dtout_drin,0,0,dtout_dtin,0,0,rt_in,0) +void mrcal_invert_rt_full( // output + double* rt_out, // (6,) array + int rt_out_stride0, // in bytes. <= 0 means "contiguous" + double* dtout_drin, // (3,3) array + int dtout_drin_stride0, // in bytes. <= 0 means "contiguous" + int dtout_drin_stride1, // in bytes. <= 0 means "contiguous" + double* dtout_dtin, // (3,3) array + int dtout_dtin_stride0, // in bytes. <= 0 means "contiguous" + int dtout_dtin_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt_in, // (6,) array + int rt_in_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Compose two Rt transformations +// +// Rt = Rt0 * Rt1 ---> Rt(x) = Rt0( Rt1(x) ) +// +// The input transformations are given in (4,3) arrays Rt_0 and Rt_1 +// +// The result is returned in a (4,3) array Rt_out +// +// In-place operation is supported; the output array may be the same as either +// of the input arrays to overwrite the input. +#define mrcal_compose_Rt(Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0) +void mrcal_compose_Rt_full( // output + double* Rt_out, // (4,3) array + int Rt_out_stride0, // in bytes. <= 0 means "contiguous" + int Rt_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt_0, // (4,3) array + int Rt_0_stride0, // in bytes. <= 0 means "contiguous" + int Rt_0_stride1, // in bytes. <= 0 means "contiguous" + const double* Rt_1, // (4,3) array + int Rt_1_stride0, // in bytes. <= 0 means "contiguous" + int Rt_1_stride1 // in bytes. <= 0 means "contiguous" + ); + +// Compose two rt transformations +// +// rt = rt0 * rt1 ---> rt(x) = rt0( rt1(x) ) +// +// The input transformations are given in (6,) arrays rt_0 and rt_1 +// +// The result is returned in a (6,) array rt_out +// +// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this +// is not wanted +// +// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this +// is not wanted +// +// The gradient dt/dr0 is returned in a (3,3) array dt_dr0. Set to NULL if this +// is not wanted +// +// The gradient dt/dt1 is returned in a (3,3) array dt_dt1. Set to NULL if this +// is not wanted +// +// The gradients dr/dt0, dr/dt1, dt/dr1 are always 0, so they are never returned +// +// The gradient dt/dt0 is always identity, so it is never returned +// +// In-place operation is supported; the output array may be the same as either +// of the input arrays to overwrite the input. +#define mrcal_compose_rt(rt_out,dr_dr0,dr_dr1,dt_dr0,dt_dt1,rt_0,rt_1) mrcal_compose_rt_full(rt_out,0,dr_dr0,0,0,dr_dr1,0,0,dt_dr0,0,0,dt_dt1,0,0,rt_0,0,rt_1,0) +void mrcal_compose_rt_full( // output + double* rt_out, // (6,) array + int rt_out_stride0, // in bytes. <= 0 means "contiguous" + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + 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" + double* dt_dr0, // (3,3) array; may be NULL + int dt_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dt_dr0_stride1, // in bytes. <= 0 means "contiguous" + double* dt_dt1, // (3,3) array; may be NULL + int dt_dt1_stride0, // in bytes. <= 0 means "contiguous" + int dt_dt1_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt_0, // (6,) array + int rt_0_stride0, // in bytes. <= 0 means "contiguous" + const double* rt_1, // (6,) array + int rt_1_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Compose two angle-axis rotations +// +// r = r0 * r1 ---> r(x) = r0( r1(x) ) +// +// The input rotations are given in (3,) arrays r_0 and r_1 +// +// The result is returned in a (3,) array r_out +// +// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this +// is not wanted +// +// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this +// is not wanted +// +// In-place operation is supported; the output array may be the same as either +// of the input arrays to overwrite the input. +#define mrcal_compose_r(r_out,dr_dr0,dr_dr1,r_0,r_1) mrcal_compose_r_full(r_out,0,dr_dr0,0,0,dr_dr1,0,0,r_0,0,r_1,0) +void mrcal_compose_r_full( // output + double* r_out, // (3,) array + int r_out_stride0, // in bytes. <= 0 means "contiguous" + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + 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" + const double* r_1, // (3,) array + int r_1_stride0 // in bytes. <= 0 means "contiguous" + ); + +// Special-case rotation compositions for the uncertainty computation +// +// Same as mrcal_compose_r() except +// +// - r0 is assumed to be 0, so we don't ingest it, and we don't report the +// composition result +// - we ONLY report the dr01/dr0 gradient +// +// In python this function is equivalent to +// +// _,dr01_dr0,_ = compose_r(np.zeros((3,),), +// r1, +// get_gradients=True) +#define mrcal_compose_r_tinyr0_gradientr0(dr_dr0,r_1) \ + mrcal_compose_r_tinyr0_gradientr0_full(dr_dr0,0,0,r_1,0) +void mrcal_compose_r_tinyr0_gradientr0_full( // output + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + + // input + 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" + ); + +// Procrustes fit functions. Align two corresponding sets of normalized +// direction vectors or points. Return true on success +bool mrcal_align_procrustes_vectors_R01(// out + double* R01, + // in + const int N, + // (N,3) arrays + // normalized direction vectors + const double* v0, + const double* v1, + + // (N,) array; may be NULL to use an even + // weighting + const double* weights); + +bool mrcal_align_procrustes_points_Rt01(// out + double* Rt01, + // in + const int N, + // (N,3) arrays + const double* p0, + const double* p1, + + // (N,) array; may be NULL to use an even + // weighting + const double* weights); + +// Compute a non-unique rotation to map a given vector to [0,0,1] +void mrcal_R_aligned_to_vector(// out + double* R, + // in + const double* v); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/scales.h b/wpical/src/main/native/thirdparty/mrcal/include/scales.h new file mode 100644 index 00000000000..14ed88ab382 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/scales.h @@ -0,0 +1,43 @@ +#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/stereo.h b/wpical/src/main/native/thirdparty/mrcal/include/stereo.h new file mode 100644 index 00000000000..793523ab985 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/stereo.h @@ -0,0 +1,129 @@ +// 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 + +#pragma once + +#include "mrcal-types.h" +#include "mrcal-image.h" + +// The reference implementation in Python is _rectified_resolution_python() in +// stereo.py +// +// The Python wrapper is mrcal.rectified_resolution(), and the documentation is +// in the docstring of that function +bool mrcal_rectified_resolution( // output and input + // > 0: use given value + // < 0: autodetect and scale + double* pixels_per_deg_az, + double* pixels_per_deg_el, + + // input + const mrcal_lensmodel_t* lensmodel, + const double* intrinsics, + const mrcal_point2_t* azel_fov_deg, + const mrcal_point2_t* azel0_deg, + const double* R_cam0_rect0, + const mrcal_lensmodel_type_t rectification_model_type); + +// The reference implementation in Python is _rectified_system_python() in +// stereo.py +// +// The Python wrapper is mrcal.rectified_system(), and the documentation is in +// the docstring of that function +bool mrcal_rectified_system(// output + unsigned int* imagersize_rectified, + double* fxycxy_rectified, + double* rt_rect0_ref, + double* baseline, + + // input, output + // > 0: use given value + // < 0: autodetect and scale + double* pixels_per_deg_az, + double* pixels_per_deg_el, + + // input, output + // if(..._autodetect) { the results are returned here } + mrcal_point2_t* azel_fov_deg, + mrcal_point2_t* azel0_deg, + + // input + const mrcal_lensmodel_t* lensmodel0, + const double* intrinsics0, + + const double* rt_cam0_ref, + const double* rt_cam1_ref, + + const mrcal_lensmodel_type_t rectification_model_type, + + bool az0_deg_autodetect, + bool el0_deg_autodetect, + bool az_fov_deg_autodetect, + bool el_fov_deg_autodetect); + +// The reference implementation in Python is _rectification_maps_python() in +// stereo.py +// +// The Python wrapper is mrcal.rectification_maps(), and the documentation is in +// the docstring of that function +bool mrcal_rectification_maps(// output + // Dense array of shape (Ncameras=2, Nel, Naz, Nxy=2) + float* rectification_maps, + + // input + const mrcal_lensmodel_t* lensmodel0, + const double* intrinsics0, + const double* r_cam0_ref, + + const mrcal_lensmodel_t* lensmodel1, + const double* intrinsics1, + const double* r_cam1_ref, + + const mrcal_lensmodel_type_t rectification_model_type, + const double* fxycxy_rectified, + const unsigned int* imagersize_rectified, + const double* r_rect0_ref); + + +// The reference implementation in Python is _stereo_range_python() in +// stereo.py +// +// The Python wrapper is mrcal.stereo_range(), and the documentation is in +// the docstring of that function +bool mrcal_stereo_range_sparse(// output + double* range, // N of these + + // input + const double* disparity, // N of these + const mrcal_point2_t* qrect0, // N of these + const int N, // how many points + + const double disparity_min, + const double disparity_max, + + // models_rectified + const mrcal_lensmodel_type_t rectification_model_type, + const double* fxycxy_rectified, + const double baseline); + +bool mrcal_stereo_range_dense(// output + mrcal_image_double_t* range, + + // input + const mrcal_image_uint16_t* disparity_scaled, + const uint16_t disparity_scale, + + // Used to detect invalid values. Set to + // 0,UINT16_MAX to ignore + const uint16_t disparity_scaled_min, + const uint16_t disparity_scaled_max, + + // models_rectified + const mrcal_lensmodel_type_t rectification_model_type, + const double* fxycxy_rectified, + const double baseline); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/strides.h b/wpical/src/main/native/thirdparty/mrcal/include/strides.h new file mode 100644 index 00000000000..2c8ebf27f29 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/strides.h @@ -0,0 +1,42 @@ +// 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 + +#pragma once + +// This is an internal header to make the stride logic work. Not to be seen by +// the end-users or installed + +// stride-aware matrix access +#define _P1(x, stride0, i0) \ + (*(double*)( (char*)(x) + \ + (i0) * (stride0))) +#define _P2(x, stride0, stride1, i0, i1) \ + (*(double*)( (char*)(x) + \ + (i0) * (stride0) + \ + (i1) * (stride1))) +#define _P3(x, stride0, stride1, stride2,i0, i1, i2) \ + (*(double*)( (char*)(x) + \ + (i0) * (stride0) + \ + (i1) * (stride1) + \ + (i2) * (stride2))) + +#define P1(x, i0) _P1(x, x##_stride0, i0) +#define P2(x, i0,i1) _P2(x, x##_stride0, x##_stride1, i0,i1) +#define P3(x, i0,i1,i2) _P3(x, x##_stride0, x##_stride1, x##_stride2, i0,i1,i2) + +// Init strides. If a given stride is <= 0, set the default, as we would have if +// the data was contiguous +#define init_stride_1D(x, d0) \ + if( x ## _stride0 <= 0) x ## _stride0 = sizeof(*x) +#define init_stride_2D(x, d0, d1) \ + if( x ## _stride1 <= 0) x ## _stride1 = sizeof(*x); \ + if( x ## _stride0 <= 0) x ## _stride0 = d1 * x ## _stride1 +#define init_stride_3D(x, d0, d1, d2) \ + if( x ## _stride2 <= 0) x ## _stride2 = sizeof(*x); \ + if( x ## _stride1 <= 0) x ## _stride1 = d2 * x ## _stride2; \ + if( x ## _stride0 <= 0) x ## _stride0 = d1 * x ## _stride1 diff --git a/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h b/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h new file mode 100644 index 00000000000..cb685f95000 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/triangulation.h @@ -0,0 +1,153 @@ +// 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 + +#pragma once + +#include "basic-geometry.h" + +// All of these return (0,0,0) if the rays are parallel or divergent, or if the +// intersection is behind either of the two cameras. No gradients are reported +// in that case + +// Basic closest-approach-in-3D routine. This is the "Mid" method in +// "Triangulation: Why Optimize?", Seong Hun Lee and Javier Civera +// https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_geometric(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_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); + +// Minimize L2 pinhole reprojection error. Described in "Triangulation Made +// Easy", Peter Lindstrom, IEEE Conference on Computer Vision and Pattern +// Recognition, 2010. This is the "L2 img 5-iteration" method (but with only 2 +// iterations) in "Triangulation: Why Optimize?", Seong Hun Lee and Javier +// Civera. https://arxiv.org/abs/1907.11917 +// Lindstrom's paper recommends 2 iterations +mrcal_point3_t +mrcal_triangulate_lindstrom(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_dRt01, + + // inputs + + // not-necessarily normalized vectors in the LOCAL + // coordinate system. This is different from the other + // triangulation routines + const mrcal_point3_t* v0_local, + const mrcal_point3_t* v1_local, + const mrcal_point3_t* Rt01); + +// Minimize L1 angle error. Described in "Closed-Form Optimal Two-View +// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV +// 2019. This is the "L1 ang" method in "Triangulation: Why Optimize?", Seong +// Hun Lee and Javier Civera. https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_leecivera_l1(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_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); + +// Minimize L-infinity angle error. Described in "Closed-Form Optimal Two-View +// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV +// 2019. This is the "L-infinity ang" method in "Triangulation: Why Optimize?", +// Seong Hun Lee and Javier Civera. https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_leecivera_linf(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_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); + +// The "Mid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier +// Civera. https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_leecivera_mid2(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_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); + +// The "wMid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier +// Civera. https://arxiv.org/abs/1907.11917 +mrcal_point3_t +mrcal_triangulate_leecivera_wmid2(// outputs + // These all may be NULL + mrcal_point3_t* dm_dv0, + mrcal_point3_t* dm_dv1, + mrcal_point3_t* dm_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); + +// 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/include/util.h b/wpical/src/main/native/thirdparty/mrcal/include/util.h new file mode 100644 index 00000000000..bd151891efb --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/include/util.h @@ -0,0 +1,13 @@ +// 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 + +#pragma once + +#include + +#define MSG(fmt, ...) fprintf(stderr, "%s(%d): " fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__) diff --git a/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp b/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp new file mode 100644 index 00000000000..1717c3e7842 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp @@ -0,0 +1,332 @@ +// 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 +#include + +#include "autodiff.hh" + +extern "C" { +#include "cahvore.h" +} + +template +static +bool _project_cahvore_internals( // outputs + vec_withgrad_t* pdistorted, + + // inputs + const vec_withgrad_t& p, + const val_withgrad_t& alpha, + const val_withgrad_t& beta, + const val_withgrad_t& r0, + const val_withgrad_t& r1, + const val_withgrad_t& r2, + const val_withgrad_t& e0, + const val_withgrad_t& e1, + const val_withgrad_t& e2, + const double cahvore_linearity) +{ + // Apply a CAHVORE warp to an un-distorted point + + // Given intrinsic parameters of a CAHVORE model and a set of + // camera-coordinate points, return the projected point(s) + + // This comes from cmod_cahvore_3d_to_2d_general() in + // m-jplv/libcmod/cmod_cahvore.c + // + // The lack of documentation here comes directly from the lack of + // documentation in that function. + + // I parametrize the optical axis such that + // - o(alpha=0, beta=0) = (0,0,1) i.e. the optical axis is at the center + // if both parameters are 0 + // - The gradients are cartesian. I.e. do/dalpha and do/dbeta are both + // NOT 0 at (alpha=0,beta=0). This would happen at the poles (gimbal + // lock), and that would make my solver unhappy + // So o = { s_al*c_be, s_be, c_al*c_be } + vec_withgrad_t sca = alpha.sincos(); + vec_withgrad_t scb = beta .sincos(); + + vec_withgrad_t o; + o[0] = scb[1]*sca[0]; + o[1] = scb[0]; + o[2] = scb[1]*sca[1]; + + // Note: CAHVORE is noncentral: project(p) and project(k*p) do NOT + // project to the same point + + // What is called "omega" in the canonical CAHVOR implementation is + // called "zeta" in the canonical CAHVORE implementation. They're the + // same thing + + // cos( angle between p and o ) = inner(p,o) / (norm(o) * norm(p)) = + // zeta/norm(p) + val_withgrad_t zeta = p.dot(o); + + // Basic Computations + // Calculate initial terms + vec_withgrad_t u = o*zeta; + vec_withgrad_t ll = p - u; + val_withgrad_t l = ll.mag(); + + // Calculate theta using Newton's Method + val_withgrad_t theta = l.atan2(zeta); + + int inewton; + for( inewton = 100; inewton; inewton--) + { + // Compute terms from the current value of theta + vec_withgrad_t scth = theta.sincos(); + + val_withgrad_t theta2 = theta * theta; + val_withgrad_t theta3 = theta * theta2; + val_withgrad_t theta4 = theta * theta3; + val_withgrad_t upsilon = + zeta*scth[1] + l*scth[0] + + (scth[1] - 1.0 ) * (e0 + e1*theta2 + e2*theta4) + - (theta - scth[0]) * ( e1*theta*2.0 + e2*theta3*4.0); + + // Update theta + val_withgrad_t dtheta = + (zeta*scth[0] - l*scth[1] + - (theta - scth[0]) * (e0 + e1*theta2 + e2*theta4)) / upsilon; + + theta -= dtheta; + + // Check exit criterion from last update + if(fabs(dtheta.x) < 1e-8) + break; + } + if(inewton == 0) + { + fprintf(stderr, "%s(): too many iterations\n", __func__); + return false; + } + + // got a theta + + // Check the value of theta + if(theta.x * fabs(cahvore_linearity) > M_PI/2.) + { + fprintf(stderr, "%s(): theta out of bounds\n", __func__); + return false; + } + + // If we aren't close enough to use the small-angle approximation ... + if (theta.x > 1e-8) + { + // ... do more math! + val_withgrad_t linth = theta * cahvore_linearity; + val_withgrad_t chi; + if (cahvore_linearity < -1e-15) + chi = linth.sin() / cahvore_linearity; + else if (cahvore_linearity > 1e-15) + chi = linth.tan() / cahvore_linearity; + else + chi = theta; + val_withgrad_t chi2 = chi * chi; + val_withgrad_t chi3 = chi * chi2; + val_withgrad_t chi4 = chi * chi3; + + val_withgrad_t zetap = l / chi; + + val_withgrad_t mu = r0 + r1*chi2 + r2*chi4; + + vec_withgrad_t uu = o*zetap; + vec_withgrad_t vv = ll * (mu + 1.); + *pdistorted = uu + vv; + } + else + *pdistorted = p; + return true; +} + + +// Not meant to be touched by the end user. Implemented separate from mrcal.c so +// that I can get automated gradient propagation with c++ +extern "C" + +bool project_cahvore_internals( // outputs + mrcal_point3_t* __restrict pdistorted, + double* __restrict dpdistorted_dintrinsics_nocore, + double* __restrict dpdistorted_dp, + + // inputs + const mrcal_point3_t* __restrict p, + const double* __restrict intrinsics_nocore, + const double cahvore_linearity) +{ + if( dpdistorted_dintrinsics_nocore == NULL && + dpdistorted_dp == NULL ) + { + // No gradients. NGRAD in all the templates is 0 + + vec_withgrad_t<0,3> pdistortedg; + + vec_withgrad_t<0,8> ig; + ig.init_vars(intrinsics_nocore, + 0,8, // ivar0,Nvars + -1 // i_gradvec0 + ); + + vec_withgrad_t<0,3> pg; + pg.init_vars(p->xyz, + 0,3, // ivar0,Nvars + -1 // i_gradvec0 + ); + + if(!_project_cahvore_internals(&pdistortedg, + pg, + ig[0], + ig[1], + ig[2], + ig[3], + ig[4], + ig[5], + ig[6], + ig[7], + cahvore_linearity)) + return false; + + pdistortedg.extract_value(pdistorted->xyz); + return true; + } + + if( dpdistorted_dintrinsics_nocore == NULL && + dpdistorted_dp != NULL ) + { + // 3 variables: p (3 vars) + vec_withgrad_t<3,3> pdistortedg; + + vec_withgrad_t<3,8> ig; + ig.init_vars(intrinsics_nocore, + 0,8, // ivar0,Nvars + -1 // i_gradvec0 + ); + + vec_withgrad_t<3,3> pg; + pg.init_vars(p->xyz, + 0,3, // ivar0,Nvars + 0 // i_gradvec0 + ); + + if(!_project_cahvore_internals(&pdistortedg, + pg, + ig[0], + ig[1], + ig[2], + ig[3], + ig[4], + ig[5], + ig[6], + ig[7], + cahvore_linearity)) + return false; + + pdistortedg.extract_value(pdistorted->xyz); + pdistortedg.extract_grad (dpdistorted_dp, + 0,3, // ivar0,Nvars + 0, // i_gradvec0 + sizeof(double)*3, sizeof(double), + 3 // Nvars + ); + return true; + } + + if( dpdistorted_dintrinsics_nocore != NULL && + dpdistorted_dp == NULL ) + { + // 8 variables: alpha,beta,R,E (8 vars) + vec_withgrad_t<8,3> pdistortedg; + + vec_withgrad_t<8,8> ig; + ig.init_vars(intrinsics_nocore, + 0,8, // ivar0,Nvars + 0 // i_gradvec0 + ); + + vec_withgrad_t<8,3> pg; + pg.init_vars(p->xyz, + 0,3, // ivar0,Nvars + -1 // i_gradvec0 + ); + + if(!_project_cahvore_internals(&pdistortedg, + pg, + ig[0], + ig[1], + ig[2], + ig[3], + ig[4], + ig[5], + ig[6], + ig[7], + cahvore_linearity)) + return false; + + pdistortedg.extract_value(pdistorted->xyz); + pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore, + 0,8, // i_gradvec0,N_gradout + 0, // ivar0 + sizeof(double)*8, sizeof(double), + 3 // Nvars + ); + return true; + } + + if( dpdistorted_dintrinsics_nocore != NULL && + dpdistorted_dp != NULL ) + { + // 11 variables: alpha,beta,R,E (8 vars) + p (3 vars) + vec_withgrad_t<11,3> pdistortedg; + + vec_withgrad_t<11,8> ig; + ig.init_vars(intrinsics_nocore, + 0,8, // ivar0,Nvars + 0 // i_gradvec0 + ); + + vec_withgrad_t<11,3> pg; + pg.init_vars(p->xyz, + 0,3, // ivar0,Nvars + 8 // i_gradvec0 + ); + + if(!_project_cahvore_internals(&pdistortedg, + pg, + ig[0], + ig[1], + ig[2], + ig[3], + ig[4], + ig[5], + ig[6], + ig[7], + cahvore_linearity)) + return false; + + pdistortedg.extract_value(pdistorted->xyz); + pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore, + 0,8, // i_gradvec0,N_gradout + 0, // ivar0 + sizeof(double)*8, sizeof(double), + 3 // Nvars + ); + pdistortedg.extract_grad (dpdistorted_dp, + 8,3, // ivar0,Nvars + 0, // i_gradvec0 + sizeof(double)*3, sizeof(double), + 3 // Nvars + ); + return true; + } + + fprintf(stderr, "Getting here is a bug. Please report\n"); + assert(0); +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl b/wpical/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl new file mode 100755 index 00000000000..044098b9f23 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl @@ -0,0 +1,463 @@ +#!/usr/bin/env perl +use strict; +use warnings; +use feature qw(say); +use List::Util qw(min); +use List::MoreUtils qw(pairwise); + +say "// THIS IS AUTO-GENERATED BY $0. DO NOT EDIT BY HAND\n"; +say "// This contains dot products, norms, basic vector arithmetic and multiplication\n"; + +my @sizes = 2..6; + +# the dot products, norms and basic arithmetic functions take the size as an +# argument. I'm assuming that the compiler will expand these out for each +# particular invocation +dotProducts(); +norms(); +vectorArithmetic(); + +foreach my $n(@sizes) +{ + matrixVectorSym($n); + + foreach my $m (@sizes) + { + matrixVectorGen($n, $m) + } + + matrixMatrixSym($n); + matrixMatrixGen($n); +} + +# this is only defined for N=3. I haven't made the others yet and I don't yet need them +matrixMatrixMatrixSym(3); + + + + + + +sub dotProducts +{ + print < vout +static inline void add_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout) +{ + for(int i=0; i vout +static inline void sub_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout) +{ + for(int i=0; i 0); + + for my $i(0..$n-1) + { + my $isym_row = _getSymmetricIndices_row(\%isymHash, $i, $n); + my @cols = 0..$n-1; + + our ($a,$b); + my @sum_components = pairwise {"s[$a]*v[$b]"} @$isym_row, @cols; + $vout .= " vout[$i] = " . join(' + ', @sum_components) . ";\n"; + } + + $vout .= "}"; + + + print _multiplicationVersions($vout, $n, $n); +} + + +sub matrixVectorGen +{ + my $n = shift; + my $m = shift; + + # I now make NxM matrix-vector multiplication. I describe matrices math-style + # with the number of rows first (NxM has N rows, M columns). I store the + # matrices row-first and treat vectors as row-vectors. Thus these functons + # compute v*A where v is the row vector and A is the NxM matrix + + my $vout = <{$key} ) + { + $hash->{$key} = $hash->{next}; + $hash->{next}++; + } + + push @isym, $hash->{$key}; + } + + return \@isym; +} + +sub _getFirstDataArg +{ + my $v = shift; + + # I have a string with a bunch of functions. Get the first argument. I ASSUME + # THE FIRST ARGUMENT HAS THE SAME NAME IN ALL OF THESE + my ($arg0) = $v =~ m/^static inline.*\(.*?double\* restrict ([a-z0-9_]+),/m or die "Couldn't recognize function in '$v'"; + return $arg0; +} + +sub _makeInplace_mulVector +{ + my $v = shift; + my $arg0 = shift; + my $n = shift; + my $m = shift; + + # rename functions + $v =~ s/_vout//gm; + + # get rid of the 'vout argument' + $v =~ s/, double\* restrict vout//gm; + + # un-const first argument + $v =~ s/^(static inline.*\(.*?)const (double.*)$/$1$2/gm; + + # use the first argument instead of vout + $v =~ s/vout/$arg0/gm; + + # if we're asked to make some temporary variables, do it + if(defined $n) + { + # if no $m is given, use $m; + $m //= $n; + + my $nt = min($n-1,$m); + + # use the temporaries instead of the main variable when possible + foreach my $t(0..$nt-1) + { + $v =~ s/(=.*)${arg0}\[$t\]/$1t[$t]/mg; + } + + # define the temporaries. I need one fewer than n + my $tempDef = " double t[$nt] = {" . join(', ', map {"${arg0}[$_]"} 0..$nt-1) . "};"; + $v =~ s/^\{$/{\n$tempDef/mg; + } + + return $v; +} +sub _makeInplace_mulMatrix +{ + my $v = shift; + + # rename functions + $v =~ s/_vout//gm; + + # get rid of the 'vout argument' + $v =~ s/, double\* restrict vout//gm; + + # un-const first argument + $v =~ s/^(static inline.*\(.*?)const (double.*)$/$1$2/gm; + + # use the first argument instead of vout + $v =~ s/,[^\),]*vout[^\),]*([\),])/$1/gm; + + return $v; +} + +sub _makeVaccum +{ + my $v = shift; + + # rename functions + $v =~ s/_vout/_vaccum/gm; + + # vout -> vaccum + $v =~ s/vout/vaccum/gm; + + # make sure we accumulate + $v =~ s/(vaccum\[.*?\]\s*)=/$1+=/gm; + + # better comment + $v =~ s/-> vaccum/-> + vaccum/gm; + + return $v; +} + +sub _makeScaled_arithmetic +{ + my $f = shift; + + # rename functions + $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm; + + # add the scale argument + $f =~ s/^(static inline .*)\)$/$1, double scale)/gm; + + # apply the scaling + $f =~ s/([+-]) b/$1 scale*b/gm; + + return $f; +} + +sub _makeScaled_mulVector +{ + my $f = shift; + + # rename functions + $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm; + + # add the scale argument + $f =~ s/^(static inline .*)\)$/$1, double scale)/gm; + + # apply the scaling + $f =~ s/(.*=\s*)([^{}]*?);$/${1}scale * ($2);/gm; + + return $f; +} + +sub _makeScaled_mulMatrix +{ + my $f = shift; + + # rename functions + $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm; + + # add the scale argument + $f =~ s/^(static inline .*)\)$/$1, double scale)/gm; + + # apply the scaling. This is simply an argument to the vector function I call + $f =~ s/^(\s*mul_.*)(\).*)/$1, scale$2/gm; + + # apply the scaling. Call the _scaled vector function + $f =~ s/^(\s*mul_.*?)(\s*\()/${1}_scaled$2/gm; + + return $f; +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/mrcal.cpp b/wpical/src/main/native/thirdparty/mrcal/src/mrcal.cpp new file mode 100644 index 00000000000..5ee9337e33c --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/mrcal.cpp @@ -0,0 +1,6890 @@ +// 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 + +// Apparently I need this in MSVC to get constants +#include +#define _USE_MATH_DEFINES + +#include +#include +#include + +extern "C" { +#include +} +#include +#include +#include +#include + +extern "C" { +#include "mrcal.h" +#include "minimath/minimath.h" +#include "cahvore.h" +#include "minimath/minimath-extra.h" +#include "util.h" +#include "scales.h" +} +#define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0) + +#define restrict __restrict + +// Returns a static string, using "..." as a placeholder for any configuration +// values +#define LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + "_" #name "=..." +#define LENSMODEL_PRINT_CFG_ELEMENT_FMT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + "_" #name "=%" PRIcode +#define LENSMODEL_PRINT_CFG_ELEMENT_VAR(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + ,config->name +#define LENSMODEL_SCAN_CFG_ELEMENT_FMT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + "_" #name "=%" SCNcode +#define LENSMODEL_SCAN_CFG_ELEMENT_VAR(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + ,&config->name +#define LENSMODEL_SCAN_CFG_ELEMENT_PLUS1(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) \ + +1 +const char* mrcal_lensmodel_name_unconfigured( const mrcal_lensmodel_t* lensmodel ) +{ + switch(lensmodel->type) + { +#define CASE_STRING_NOCONFIG(s,n) case MRCAL_##s: ; \ + return #s; +#define _CASE_STRING_WITHCONFIG(s,n,s_CONFIG_LIST) case MRCAL_##s: ; \ + return #s s_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE, ); +#define CASE_STRING_WITHCONFIG(s,n) _CASE_STRING_WITHCONFIG(s,n,MRCAL_ ## s ## _CONFIG_LIST) + + MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_STRING_NOCONFIG ) + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CASE_STRING_WITHCONFIG ) + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CASE_STRING_WITHCONFIG ) + + default: + return NULL; + + +#undef CASE_STRING_NOCONFIG +#undef CASE_STRING_WITHCONFIG + + } + return NULL; +} + +// Write the model name WITH the full config into the given buffer. Identical to +// mrcal_lensmodel_name_unconfigured() for configuration-free models +static int LENSMODEL_CAHVORE__snprintf_model + (char* out, int size, + const mrcal_LENSMODEL_CAHVORE__config_t* config) +{ + return + snprintf( out, size, "LENSMODEL_CAHVORE" + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_FMT, ) + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_VAR, )); +} +static int LENSMODEL_SPLINED_STEREOGRAPHIC__snprintf_model + (char* out, int size, + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config) +{ + return + snprintf( out, size, "LENSMODEL_SPLINED_STEREOGRAPHIC" + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_FMT, ) + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_VAR, )); +} +bool mrcal_lensmodel_name( char* out, int size, const mrcal_lensmodel_t* lensmodel ) +{ + switch(lensmodel->type) + { +#define CASE_STRING_NOCONFIG(s,n) case MRCAL_##s: \ + return size > snprintf(out,size, #s); + +#define CASE_STRING_WITHCONFIG(s,n) case MRCAL_##s: \ + return size > s##__snprintf_model(out, size, &lensmodel->s##__config); + + MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_STRING_NOCONFIG ) + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CASE_STRING_WITHCONFIG ) + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CASE_STRING_WITHCONFIG ) + + default: + return NULL; + +#undef CASE_STRING_NOCONFIG +#undef CASE_STRING_WITHCONFIG + + } + return NULL; +} + + +static bool LENSMODEL_CAHVORE__scan_model_config( mrcal_LENSMODEL_CAHVORE__config_t* config, const char* config_str) +{ + int pos; + int Nelements = 0 MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_PLUS1, ); + return + Nelements == + sscanf( config_str, + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_FMT, )"%n" + MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_VAR, ), + &pos) && + config_str[pos] == '\0'; +} +static bool LENSMODEL_SPLINED_STEREOGRAPHIC__scan_model_config( mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config, const char* config_str) +{ + int pos; + int Nelements = 0 MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_PLUS1, ); + return + Nelements == + sscanf( config_str, + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_FMT, )"%n" + MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(LENSMODEL_SCAN_CFG_ELEMENT_VAR, ), + &pos) && + config_str[pos] == '\0'; +} + +const char* const* mrcal_supported_lensmodel_names( void ) +{ +#define NAMESTRING_NOCONFIG(s,n) #s, +#define _NAMESTRING_WITHCONFIG(s,n,s_CONFIG_LIST) #s s_CONFIG_LIST(LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE, ), +#define NAMESTRING_WITHCONFIG(s,n) _NAMESTRING_WITHCONFIG(s,n,MRCAL_ ## s ## _CONFIG_LIST) + + static const char* names[] = { + MRCAL_LENSMODEL_NOCONFIG_LIST( NAMESTRING_NOCONFIG) + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( NAMESTRING_WITHCONFIG) + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(NAMESTRING_WITHCONFIG) + NULL }; + return names; +} + +#undef LENSMODEL_PRINT_CFG_ELEMENT_TEMPLATE +#undef LENSMODEL_PRINT_CFG_ELEMENT_FMT +#undef LENSMODEL_PRINT_CFG_ELEMENT_VAR +#undef LENSMODEL_SCAN_CFG_ELEMENT_FMT +#undef LENSMODEL_SCAN_CFG_ELEMENT_VAR +#undef LENSMODEL_SCAN_CFG_ELEMENT_PLUS1 + +// parses the model name AND the configuration into a mrcal_lensmodel_t structure. +// On error returns false with lensmodel->type set to MRCAL_LENSMODEL_INVALID_... +bool mrcal_lensmodel_from_name( // output + mrcal_lensmodel_t* lensmodel, + + // input + const char* name ) +{ +#define CHECK_AND_RETURN_NOCONFIG(s,n) \ + if( 0 == strcmp( name, #s) ) \ + { \ + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_##s}; \ + return true; \ + } + +#define CHECK_AND_RETURN_WITHCONFIG(s,n) \ + /* Configured model. I need to extract the config from the string. */ \ + /* The string format is NAME_cfg1=var1_cfg2=var2... */ \ + { \ + const int len_s = strlen(#s); \ + if( 0 == strncmp( name, #s, len_s ) ) \ + { \ + if(name[len_s] == '\0') \ + { \ + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_MISSINGCONFIG}; \ + return false; \ + } \ + if(name[len_s] == '_') \ + { \ + /* found name. Now extract the config */ \ + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_##s}; \ + mrcal_##s##__config_t* config = &lensmodel->s##__config; \ + \ + const char* config_str = &name[len_s]; \ + \ + if(s##__scan_model_config(config, config_str)) \ + return true; \ + \ + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_BADCONFIG}; \ + return false; \ + } \ + } \ + } + + if(name == NULL) + { + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_TYPE}; + return false; + } + + MRCAL_LENSMODEL_NOCONFIG_LIST( CHECK_AND_RETURN_NOCONFIG ); + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CHECK_AND_RETURN_WITHCONFIG ); + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CHECK_AND_RETURN_WITHCONFIG ); + + *lensmodel = (mrcal_lensmodel_t){.type = MRCAL_LENSMODEL_INVALID_TYPE}; + return false; +#undef CHECK_AND_RETURN_NOCONFIG +#undef CHECK_AND_RETURN_WITHCONFIG +} + +// parses the model name only. The configuration is ignored. Even if it's +// missing or unparseable. Unknown model names return +// MRCAL_LENSMODEL_INVALID_TYPE +mrcal_lensmodel_type_t mrcal_lensmodel_type_from_name( const char* name ) +{ +#define CHECK_AND_RETURN_NOCONFIG(s,n) \ + if( 0 == strcmp( name, #s) ) return MRCAL_##s; + +#define CHECK_AND_RETURN_WITHCONFIG(s,n) \ + /* Configured model. If the name is followed by _ or nothing, I */ \ + /* accept this model */ \ + { \ + const int len_s = strlen(#s); \ + if( 0 == strncmp( name, #s, len_s) && \ + ( name[len_s] == '\0' || \ + name[len_s] == '_' ) ) \ + return MRCAL_##s; \ + } + + if(name == NULL) + return MRCAL_LENSMODEL_INVALID_TYPE; + + MRCAL_LENSMODEL_NOCONFIG_LIST( CHECK_AND_RETURN_NOCONFIG ); + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CHECK_AND_RETURN_WITHCONFIG ); + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CHECK_AND_RETURN_WITHCONFIG ); + + return MRCAL_LENSMODEL_INVALID_TYPE; + +#undef CHECK_AND_RETURN_NOCONFIG +#undef CHECK_AND_RETURN_WITHCONFIG +} + +mrcal_lensmodel_metadata_t mrcal_lensmodel_metadata( const mrcal_lensmodel_t* lensmodel ) +{ + switch(lensmodel->type) + { + case MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC: + case MRCAL_LENSMODEL_STEREOGRAPHIC: + case MRCAL_LENSMODEL_LONLAT: + case MRCAL_LENSMODEL_LATLON: + return (mrcal_lensmodel_metadata_t) { .has_core = true, + .can_project_behind_camera = true, + .has_gradients = true, + .noncentral = false}; + case MRCAL_LENSMODEL_PINHOLE: + case MRCAL_LENSMODEL_OPENCV4: + case MRCAL_LENSMODEL_OPENCV5: + case MRCAL_LENSMODEL_OPENCV8: + case MRCAL_LENSMODEL_OPENCV12: + case MRCAL_LENSMODEL_CAHVOR: + return (mrcal_lensmodel_metadata_t) { .has_core = true, + .can_project_behind_camera = false, + .has_gradients = true, + .noncentral = false }; + + case MRCAL_LENSMODEL_CAHVORE: + return (mrcal_lensmodel_metadata_t) { .has_core = true, + .can_project_behind_camera = false, + .has_gradients = true, + .noncentral = true }; + + default: ; + } + MSG("Unknown lens model %d. Barfing out", lensmodel->type); + assert(0); +} + +static +bool modelHasCore_fxfycxcy( const mrcal_lensmodel_t* lensmodel ) +{ + mrcal_lensmodel_metadata_t meta = mrcal_lensmodel_metadata(lensmodel); + return meta.has_core; +} +static +bool model_supports_projection_behind_camera( const mrcal_lensmodel_t* lensmodel ) +{ + mrcal_lensmodel_metadata_t meta = mrcal_lensmodel_metadata(lensmodel); + return meta.can_project_behind_camera; +} + +static int LENSMODEL_SPLINED_STEREOGRAPHIC__lensmodel_num_params(const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config) +{ + return + // I have two surfaces: one for x and another for y + (int)config->Nx * (int)config->Ny * 2 + + + // and I have a core + 4; +} +int mrcal_lensmodel_num_params(const mrcal_lensmodel_t* lensmodel) +{ +#define CHECK_CONFIG_NPARAM_NOCONFIG(s,n) \ + _Static_assert(n >= 0, "no-config implies known-at-compile-time param count"); + + switch(lensmodel->type) + { +#define CASE_NUM_STATIC(s,n) \ + case MRCAL_##s: return n; + +#define CASE_NUM_DYNAMIC(s,n) \ + case MRCAL_##s: return s##__lensmodel_num_params(&lensmodel->s##__config); + + MRCAL_LENSMODEL_NOCONFIG_LIST( CASE_NUM_STATIC ) + MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST( CASE_NUM_STATIC ) + MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST( CASE_NUM_DYNAMIC ) + + default: ; + } + return -1; + +#undef CASE_NUM_NOCONFIG +#undef CASE_NUM_WITHCONFIG +} + +static +int get_num_distortions_optimization_params(mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if( !problem_selections.do_optimize_intrinsics_distortions ) + return 0; + + int N = mrcal_lensmodel_num_params(lensmodel); + if(modelHasCore_fxfycxcy(lensmodel)) + N -= 4; // ignoring fx,fy,cx,cy + return N; +} + +int mrcal_num_intrinsics_optimization_params(mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + int N = get_num_distortions_optimization_params(problem_selections, lensmodel); + + if( problem_selections.do_optimize_intrinsics_core && + modelHasCore_fxfycxcy(lensmodel) ) + N += 4; // fx,fy,cx,cy + return N; +} + +int mrcal_num_states(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + mrcal_num_states_extrinsics(Ncameras_extrinsics, + problem_selections) + + mrcal_num_states_frames(Nframes, + problem_selections) + + mrcal_num_states_points(Npoints, Npoints_fixed, + problem_selections) + + mrcal_num_states_calobject_warp( problem_selections, + Nobservations_board); +} + +static int num_regularization_terms_percamera(mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(!problem_selections.do_apply_regularization) + return 0; + + // distortions + int N = get_num_distortions_optimization_params(problem_selections, lensmodel); + // optical center + if(problem_selections.do_optimize_intrinsics_core) + N += 2; + return N; +} + +int mrcal_measurement_index_boards(int i_observation_board, + int Nobservations_board, + int Nobservations_point, + int calibration_object_width_n, + int calibration_object_height_n) +{ + if(Nobservations_board <= 0) + return -1; + + // *2 because I have separate x and y measurements + return + 0 + + mrcal_num_measurements_boards(i_observation_board, + calibration_object_width_n, + calibration_object_height_n); +} + +int mrcal_num_measurements_boards(int Nobservations_board, + int calibration_object_width_n, + int calibration_object_height_n) +{ + if(Nobservations_board <= 0) + return 0; + + // *2 because I have separate x and y measurements + return + Nobservations_board * + calibration_object_width_n*calibration_object_height_n * + 2; + + + return mrcal_measurement_index_boards( Nobservations_board, + 0,0, + calibration_object_width_n, + calibration_object_height_n); +} + +int mrcal_measurement_index_points(int i_observation_point, + int Nobservations_board, + int Nobservations_point, + int calibration_object_width_n, + int calibration_object_height_n) +{ + if(Nobservations_point <= 0) + return -1; + + // 2: x,y measurements + return + mrcal_num_measurements_boards(Nobservations_board, + calibration_object_width_n, + calibration_object_height_n) + + i_observation_point * 2; +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: need known-range points, at-infinity points" +#endif + +int mrcal_num_measurements_points(int Nobservations_point) +{ + // 2: x,y measurements + return Nobservations_point * 2; +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: Add a test for mrcal_measurement_index_points_triangulated()" +#endif +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); +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: python-wrap this function" +#endif +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 ); +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: python-wrap this function" +#endif +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; +} + + +int mrcal_measurement_index_regularization( +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: this argument order is weird. Put then triangulated stuff at the end?" +#endif + // 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, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, int Nobservations_point, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + + if(mrcal_num_measurements_regularization(Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, + problem_selections, + lensmodel) <= 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(observations_point_triangulated, + Nobservations_point_triangulated); +} + +int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + 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); +} + +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, + int Nframes, + int Npoints, int Npoints_fixed, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + return + mrcal_num_measurements_boards( Nobservations_board, + calibration_object_width_n, + calibration_object_height_n) + + mrcal_num_measurements_points(Nobservations_point) + + mrcal_num_measurements_points_triangulated(observations_point_triangulated, + Nobservations_point_triangulated) + + mrcal_num_measurements_regularization(Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, + problem_selections, + lensmodel); +} + +static bool has_calobject_warp(mrcal_problem_selections_t problem_selections, + int Nobservations_board) +{ + return problem_selections.do_optimize_calobject_warp && Nobservations_board>0; +} + +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, + int Nframes, + int Npoints, int Npoints_fixed, + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + // each observation depends on all the parameters for THAT frame and for + // THAT camera. Camera0 doesn't have extrinsics, so I need to loop through + // all my observations + + // Each projected point has an x and y measurement, and each one depends on + // some number of the intrinsic parameters. Parametric models are simple: + // each one depends on ALL of the intrinsics. Splined models are sparse, + // however, and there's only a partial dependence + int Nintrinsics_per_measurement; + if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + int run_len = + lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.order + 1; + Nintrinsics_per_measurement = + (problem_selections.do_optimize_intrinsics_core ? 4 : 0) + + (problem_selections.do_optimize_intrinsics_distortions ? (run_len*run_len) : 0); + } + else + Nintrinsics_per_measurement = + mrcal_num_intrinsics_optimization_params(problem_selections, lensmodel); + + // x depends on fx,cx but NOT on fy, cy. And similarly for y. + if( problem_selections.do_optimize_intrinsics_core && + modelHasCore_fxfycxcy(lensmodel) ) + Nintrinsics_per_measurement -= 2; + + int N = Nobservations_board * ( (problem_selections.do_optimize_frames ? 6 : 0) + + (problem_selections.do_optimize_extrinsics ? 6 : 0) + + (has_calobject_warp(problem_selections,Nobservations_board) ? MRCAL_NSTATE_CALOBJECT_WARP : 0) + + Nintrinsics_per_measurement ); + + // initial estimate counts extrinsics for the reference camera, which need + // to be subtracted off + if(problem_selections.do_optimize_extrinsics) + for(int i=0; i= 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); + } + } + + // Regularization + if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + if(problem_selections.do_apply_regularization) + { + // Each regularization term depends on + // - two values for distortions + // - one value for the center pixel + N += + Ncameras_intrinsics * + 2 * + num_regularization_terms_percamera(problem_selections, + lensmodel); + // I multiplied by 2, so I double-counted the center pixel + // contributions. Subtract those off + if(problem_selections.do_optimize_intrinsics_core) + N -= Ncameras_intrinsics*2; + } + } + else + N += + Ncameras_intrinsics * + num_regularization_terms_percamera(problem_selections, + lensmodel); + + if(problem_selections.do_apply_regularization_unity_cam01 && + problem_selections.do_optimize_extrinsics && + Ncameras_extrinsics > 0) + { +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: I assume that camera0 is at the reference" +#endif + N += 3; // translation only + } + + return N; +} + +// Used in the spline-based projection function. +// +// See bsplines.py for the derivation of the spline expressions and for +// justification of the 2D scheme +// +// Here we sample two interpolated surfaces at once: one each for the x and y +// focal-length scales +// +// The sampling function assumes evenly spaced knots. +// a,b,c,d are sequential control points +// x is in [0,1] between b and c. Function looks like this: +// double A = fA(x); +// double B = fB(x); +// double C = fC(x); +// double D = fD(x); +// return A*a + B*b + C*c + D*d; +// I need to sample many such 1D segments, so I compute A,B,C,D separately, +// and apply them together +static +void get_sample_coeffs__cubic(double* ABCD, double* ABCDgrad, double x) +{ + double x2 = x*x; + double x3 = x2*x; + ABCD[0] = (-x3 + 3*x2 - 3*x + 1)/6; + ABCD[1] = (3 * x3/2 - 3*x2 + 2)/3; + ABCD[2] = (-3 * x3 + 3*x2 + 3*x + 1)/6; + ABCD[3] = x3 / 6; + + ABCDgrad[0] = -x2/2 + x - 1./2.; + ABCDgrad[1] = 3*x2/2 - 2*x; + ABCDgrad[2] = -3*x2/2 + x + 1./2.; + ABCDgrad[3] = x2 / 2; +} +static +void interp__cubic(double* out, const double* ABCDx, const double* ABCDy, + // control points + const double* c, + int stridey) +{ + double cinterp[4][2]; + const int stridex = 2; + for(int iy=0; iy<4; iy++) + for(int k=0;k<2;k++) + cinterp[iy][k] = + ABCDx[0] * c[iy*stridey + 0*stridex + k] + + ABCDx[1] * c[iy*stridey + 1*stridex + k] + + ABCDx[2] * c[iy*stridey + 2*stridex + k] + + ABCDx[3] * c[iy*stridey + 3*stridex + k]; + for(int k=0;k<2;k++) + out[k] = + ABCDy[0] * cinterp[0][k] + + ABCDy[1] * cinterp[1][k] + + ABCDy[2] * cinterp[2][k] + + ABCDy[3] * cinterp[3][k]; +} +static +void sample_bspline_surface_cubic(double* out, + double* dout_dx, + double* dout_dy, + double* ABCDx_ABCDy, + + double x, double y, + // control points + const double* c, + int stridey + + // stridex is 2: the control points from the + // two surfaces are next to each other. Better + // cache locality maybe + ) +{ + double* ABCDx = &ABCDx_ABCDy[0]; + double* ABCDy = &ABCDx_ABCDy[4]; + + // 4 samples along one dimension, and then one sample along the other + // dimension, using the 4 samples as the control points. Order doesn't + // matter. See bsplines.py + // + // I do this twice: one for each focal length surface + double ABCDgradx[4]; + double ABCDgrady[4]; + get_sample_coeffs__cubic(ABCDx, ABCDgradx, x); + get_sample_coeffs__cubic(ABCDy, ABCDgrady, y); + + // the intrinsics gradient is flatten(ABCDx[0..3] * ABCDy[0..3]) for both x + // and y. By returning ABCD[xy] and not the cartesian products, I make + // smaller temporary data arrays + interp__cubic(out, ABCDx, ABCDy, c, stridey); + interp__cubic(dout_dx, ABCDgradx, ABCDy, c, stridey); + interp__cubic(dout_dy, ABCDx, ABCDgrady, c, stridey); +} +// The sampling function assumes evenly spaced knots. +// a,b,c are sequential control points +// x is in [-1/2,1/2] around b. Function looks like this: +// double A = fA(x); +// double B = fB(x); +// double C = fC(x); +// return A*a + B*b + C*c; +// I need to sample many such 1D segments, so I compute A,B,C separately, +// and apply them together +static +void get_sample_coeffs__quadratic(double* ABC, double* ABCgrad, double x) +{ + double x2 = x*x; + ABC[0] = (4*x2 - 4*x + 1)/8; + ABC[1] = (3 - 4*x2)/4; + ABC[2] = (4*x2 + 4*x + 1)/8; + + ABCgrad[0] = x - 1./2.; + ABCgrad[1] = -2.*x; + ABCgrad[2] = x + 1./2.; +} +static +void interp__quadratic(double* out, const double* ABCx, const double* ABCy, + // control points + const double* c, + int stridey) +{ + double cinterp[3][2]; + const int stridex = 2; + for(int iy=0; iy<3; iy++) + for(int k=0;k<2;k++) + cinterp[iy][k] = + ABCx[0] * c[iy*stridey + 0*stridex + k] + + ABCx[1] * c[iy*stridey + 1*stridex + k] + + ABCx[2] * c[iy*stridey + 2*stridex + k]; + for(int k=0;k<2;k++) + out[k] = + ABCy[0] * cinterp[0][k] + + ABCy[1] * cinterp[1][k] + + ABCy[2] * cinterp[2][k]; +} +static +void sample_bspline_surface_quadratic(double* out, + double* dout_dx, + double* dout_dy, + double* ABCx_ABCy, + + double x, double y, + // control points + const double* c, + int stridey + + // stridex is 2: the control points from the + // two surfaces are next to each other. Better + // cache locality maybe + ) +{ + double* ABCx = &ABCx_ABCy[0]; + double* ABCy = &ABCx_ABCy[3]; + + // 3 samples along one dimension, and then one sample along the other + // dimension, using the 3 samples as the control points. Order doesn't + // matter. See bsplines.py + // + // I do this twice: one for each focal length surface + double ABCgradx[3]; + double ABCgrady[3]; + get_sample_coeffs__quadratic(ABCx, ABCgradx, x); + get_sample_coeffs__quadratic(ABCy, ABCgrady, y); + + // the intrinsics gradient is flatten(ABCx[0..3] * ABCy[0..3]) for both x + // and y. By returning ABC[xy] and not the cartesian products, I make + // smaller temporary data arrays + interp__quadratic(out, ABCx, ABCy, c, stridey); + interp__quadratic(dout_dx, ABCgradx, ABCy, c, stridey); + interp__quadratic(dout_dy, ABCx, ABCgrady, c, stridey); +} + +typedef struct +{ + double _d_rj_rf[3*3]; + double _d_rj_rc[3*3]; + double _d_tj_tf[3*3]; + double _d_tj_rc[3*3]; + + // _d_tj_tc is always identity + // _d_tj_rf is always 0 + // _d_rj_tf is always 0 + // _d_rj_tc is always 0 + +} geometric_gradients_t; + +static +void project_cahvor( // outputs + mrcal_point2_t* restrict q, + mrcal_point2_t* restrict dq_dfxy, + double* restrict dq_dintrinsics_nocore, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + + // inputs + const mrcal_point3_t* restrict p, + const mrcal_point3_t* restrict dp_drc, + const mrcal_point3_t* restrict dp_dtc, + const mrcal_point3_t* restrict dp_drf, + const mrcal_point3_t* restrict dp_dtf, + + const double* restrict intrinsics, + bool camera_at_identity, + const mrcal_lensmodel_t* lensmodel) +{ + int NdistortionParams = mrcal_lensmodel_num_params(lensmodel) - 4; + + // I perturb p, and then apply the focal length, center pixel stuff + // normally + mrcal_point3_t p_distorted; + + bool need_any_extrinsics_gradients = + ( dq_drcamera != NULL ) || + ( dq_dtcamera != NULL ) || + ( dq_drframe != NULL ) || + ( dq_dtframe != NULL ); + + // distortion parameter layout: + // alpha + // beta + // r0 + // r1 + // r2 + double alpha = intrinsics[4 + 0]; + double beta = intrinsics[4 + 1]; + double r0 = intrinsics[4 + 2]; + double r1 = intrinsics[4 + 3]; + double r2 = intrinsics[4 + 4]; + + double s_al = sin(alpha); + double c_al = cos(alpha); + double s_be = sin(beta); + double c_be = cos(beta); + + // I parametrize the optical axis such that + // - o(alpha=0, beta=0) = (0,0,1) i.e. the optical axis is at the center + // if both parameters are 0 + // - The gradients are cartesian. I.e. do/dalpha and do/dbeta are both + // NOT 0 at (alpha=0,beta=0). This would happen at the poles (gimbal + // lock), and that would make my solver unhappy + double o [] = { s_al*c_be, s_be, c_al*c_be }; + double do_dalpha[] = { c_al*c_be, 0, -s_al*c_be }; + double do_dbeta[] = { -s_al*s_be, c_be, -c_al*s_be }; + + double norm2p = norm2_vec(3, p->xyz); + double omega = dot_vec(3, p->xyz, o); + double domega_dalpha = dot_vec(3, p->xyz, do_dalpha); + double domega_dbeta = dot_vec(3, p->xyz, do_dbeta); + + double omega_recip = 1.0 / omega; + double tau = norm2p * omega_recip*omega_recip - 1.0; + double s__dtau_dalphabeta__domega_dalphabeta = -2.0*norm2p * omega_recip*omega_recip*omega_recip; + double dmu_dtau = r1 + 2.0*tau*r2; + double dmu_dxyz[3]; + for(int i=0; i<3; i++) + dmu_dxyz[i] = dmu_dtau * + (2.0 * p->xyz[i] * omega_recip*omega_recip + s__dtau_dalphabeta__domega_dalphabeta * o[i]); + double mu = r0 + tau*r1 + tau*tau*r2; + double s__dmu_dalphabeta__domega_dalphabeta = dmu_dtau * s__dtau_dalphabeta__domega_dalphabeta; + + double dpdistorted_dpcam[3*3] = {}; + std::vector dpdistorted_ddistortion(3*NdistortionParams); + + for(int i=0; i<3; i++) + { + double dmu_ddist[5] = { s__dmu_dalphabeta__domega_dalphabeta * domega_dalpha, + s__dmu_dalphabeta__domega_dalphabeta * domega_dbeta, + 1.0, + tau, + tau * tau }; + + dpdistorted_ddistortion[i*NdistortionParams + 0] = p->xyz[i] * dmu_ddist[0]; + dpdistorted_ddistortion[i*NdistortionParams + 1] = p->xyz[i] * dmu_ddist[1]; + dpdistorted_ddistortion[i*NdistortionParams + 2] = p->xyz[i] * dmu_ddist[2]; + dpdistorted_ddistortion[i*NdistortionParams + 3] = p->xyz[i] * dmu_ddist[3]; + dpdistorted_ddistortion[i*NdistortionParams + 4] = p->xyz[i] * dmu_ddist[4]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= dmu_ddist[0] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= dmu_ddist[1] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 2] -= dmu_ddist[2] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 3] -= dmu_ddist[3] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 4] -= dmu_ddist[4] * omega*o[i]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= mu * domega_dalpha*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= mu * domega_dbeta *o[i]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= mu * omega * do_dalpha[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= mu * omega * do_dbeta [i]; + + dpdistorted_dpcam[3*i + i] = mu+1.0; + for(int j=0; j<3; j++) + { + dpdistorted_dpcam[3*i + j] += (p->xyz[i] - omega*o[i]) * dmu_dxyz[j]; + dpdistorted_dpcam[3*i + j] -= mu*o[i]*o[j]; + } + + p_distorted.xyz[i] = p->xyz[i] + mu * (p->xyz[i] - omega*o[i]); + } + + // q = fxy pxy/pz + cxy + // dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1]) + // dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1]) + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + double pz_recip = 1. / p_distorted.z; + q->x = p_distorted.x*pz_recip * fx + cx; + q->y = p_distorted.y*pz_recip * fy + cy; + + if(need_any_extrinsics_gradients) + { + double dq_dp[2][3] = + { { fx * pz_recip, 0, -fx*p_distorted.x*pz_recip*pz_recip}, + { 0, fy * pz_recip, -fy*p_distorted.y*pz_recip*pz_recip} }; + // This is for the DISTORTED p. + // dq/deee = dq/dpdistorted dpdistorted/dpundistorted dpundistorted/deee + + double dq_dpundistorted[6]; + mul_genN3_gen33_vout(2, (double*)dq_dp, dpdistorted_dpcam, dq_dpundistorted); + + // dq/deee = dq/dp dp/deee + if(camera_at_identity) + { + if( dq_drcamera != NULL ) memset(dq_drcamera, 0, 6*sizeof(double)); + if( dq_dtcamera != NULL ) memset(dq_dtcamera, 0, 6*sizeof(double)); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe); + if( dq_dtframe != NULL ) memcpy(dq_dtframe, dq_dpundistorted, 6*sizeof(double)); + } + else + { + if( dq_drcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drc, (double*)dq_drcamera); + if( dq_dtcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtc, (double*)dq_dtcamera); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe ); + if( dq_dtframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtf, (double*)dq_dtframe ); + } + } + + if( dq_dintrinsics_nocore != NULL ) + { + for(int i=0; ix = (q->x - cx)/fx; // dqx/dfx + dq_dfxy->y = (q->y - cy)/fy; // dqy/dfy + } +} + +static +bool project_cahvore( // outputs + mrcal_point2_t* restrict q, + mrcal_point2_t* restrict dq_dfxy, + double* restrict dq_dintrinsics_nocore, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + + // inputs + const mrcal_point3_t* restrict p, + const mrcal_point3_t* restrict dp_drc, + const mrcal_point3_t* restrict dp_dtc, + const mrcal_point3_t* restrict dp_drf, + const mrcal_point3_t* restrict dp_dtf, + + const double* restrict intrinsics, + bool camera_at_identity, + const mrcal_lensmodel_t* lensmodel) +{ + int NdistortionParams = mrcal_lensmodel_num_params(lensmodel) - 4; // This is 8: alpha,beta,R,E + + mrcal_point3_t p_distorted; + double dpdistorted_ddistortion[8*3]; // 8 intrinsics parameters, nvec(p)=3 + double dpdistorted_dpcam[3*3]; + + bool need_any_extrinsics_gradients = + ( dq_drcamera != NULL ) || + ( dq_dtcamera != NULL ) || + ( dq_drframe != NULL ) || + ( dq_dtframe != NULL ); + + if(!project_cahvore_internals( // outputs + &p_distorted, + dq_dintrinsics_nocore ? dpdistorted_ddistortion : NULL, + need_any_extrinsics_gradients ? dpdistorted_dpcam : NULL, + + // inputs + p, + &intrinsics[4], + lensmodel->LENSMODEL_CAHVORE__config.linearity)) + return false; + + ////////////// exactly like in project_cahvor() above. Consolidate. + + // q = fxy pxy/pz + cxy + // dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1]) + // dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1]) + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + double pz_recip = 1. / p_distorted.z; + q->x = p_distorted.x*pz_recip * fx + cx; + q->y = p_distorted.y*pz_recip * fy + cy; + + if(need_any_extrinsics_gradients) + { + double dq_dp[2][3] = + { { fx * pz_recip, 0, -fx*p_distorted.x*pz_recip*pz_recip}, + { 0, fy * pz_recip, -fy*p_distorted.y*pz_recip*pz_recip} }; + // This is for the DISTORTED p. + // dq/deee = dq/dpdistorted dpdistorted/dpundistorted dpundistorted/deee + + double dq_dpundistorted[6]; + mul_genN3_gen33_vout(2, (double*)dq_dp, dpdistorted_dpcam, dq_dpundistorted); + + // dq/deee = dq/dp dp/deee + if(camera_at_identity) + { + if( dq_drcamera != NULL ) memset(dq_drcamera, 0, 6*sizeof(double)); + if( dq_dtcamera != NULL ) memset(dq_dtcamera, 0, 6*sizeof(double)); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe); + if( dq_dtframe != NULL ) memcpy(dq_dtframe, dq_dpundistorted, 6*sizeof(double)); + } + else + { + if( dq_drcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drc, (double*)dq_drcamera); + if( dq_dtcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtc, (double*)dq_dtcamera); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_drf, (double*)dq_drframe ); + if( dq_dtframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dpundistorted, (double*)dp_dtf, (double*)dq_dtframe ); + } + } + + if( dq_dintrinsics_nocore != NULL ) + { + for(int i=0; ix = (q->x - cx)/fx; // dqx/dfx + dq_dfxy->y = (q->y - cy)/fy; // dqy/dfy + } + return true; +} + +// These are all internals for project(). It was getting unwieldy otherwise +static +void _project_point_parametric( // outputs + mrcal_point2_t* q, + mrcal_point2_t* dq_dfxy, double* dq_dintrinsics_nocore, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + + // inputs + const mrcal_point3_t* p, + const mrcal_point3_t* dp_drc, + const mrcal_point3_t* dp_dtc, + const mrcal_point3_t* dp_drf, + const mrcal_point3_t* dp_dtf, + + const double* restrict intrinsics, + bool camera_at_identity, + const mrcal_lensmodel_t* lensmodel) +{ + // u = distort(p, distortions) + // q = uxy/uz * fxy + cxy + if(!( lensmodel->type == MRCAL_LENSMODEL_PINHOLE || + lensmodel->type == MRCAL_LENSMODEL_STEREOGRAPHIC || + lensmodel->type == MRCAL_LENSMODEL_LONLAT || + lensmodel->type == MRCAL_LENSMODEL_LATLON || + MRCAL_LENSMODEL_IS_OPENCV(lensmodel->type) )) + { + MSG("Unhandled lens model: %d (%s)", + lensmodel->type, mrcal_lensmodel_name_unconfigured(lensmodel)); + assert(0); + } + + mrcal_point3_t dq_dp[2]; + if( lensmodel->type == MRCAL_LENSMODEL_PINHOLE ) + mrcal_project_pinhole(q, dq_dp, + p, 1, intrinsics); + else if(lensmodel->type == MRCAL_LENSMODEL_STEREOGRAPHIC) + mrcal_project_stereographic(q, dq_dp, + p, 1, intrinsics); + else if(lensmodel->type == MRCAL_LENSMODEL_LONLAT) + mrcal_project_lonlat(q, dq_dp, + p, 1, intrinsics); + else if(lensmodel->type == MRCAL_LENSMODEL_LATLON) + mrcal_project_latlon(q, dq_dp, + p, 1, intrinsics); + else + { + int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + _mrcal_project_internal_opencv( q, dq_dp, + dq_dintrinsics_nocore, + p, 1, intrinsics, Nintrinsics); + } + + // dq/deee = dq/dp dp/deee + if(camera_at_identity) + { + if( dq_drcamera != NULL ) memset(dq_drcamera, 0, 6*sizeof(double)); + if( dq_dtcamera != NULL ) memset(dq_dtcamera, 0, 6*sizeof(double)); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drf, (double*)dq_drframe); + if( dq_dtframe != NULL ) memcpy(dq_dtframe, (double*)dq_dp, 6*sizeof(double)); + } + else + { + if( dq_drcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drc, (double*)dq_drcamera); + if( dq_dtcamera != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_dtc, (double*)dq_dtcamera); + if( dq_drframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_drf, (double*)dq_drframe ); + if( dq_dtframe != NULL ) mul_genN3_gen33_vout(2, (double*)dq_dp, (double*)dp_dtf, (double*)dq_dtframe ); + } + + // I have the projection, and I now need to propagate the gradients + if( dq_dfxy ) + { + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + + // I have the projection, and I now need to propagate the gradients + // xy = fxy * distort(xy)/distort(z) + cxy + dq_dfxy->x = (q->x - cx)/fx; // dqx/dfx + dq_dfxy->y = (q->y - cy)/fy; // dqy/dfy + } +} + +// Compute a pinhole projection using a constant fxy, cxy +void mrcal_project_pinhole( // output + mrcal_point2_t* q, + mrcal_point3_t* dq_dv, // May be NULL. Each point + // gets a block of 2 mrcal_point3_t + // objects + + // input + const mrcal_point3_t* v, + int N, + const double* fxycxy) +{ + const double fx = fxycxy[0]; + const double fy = fxycxy[1]; + const double cx = fxycxy[2]; + const double cy = fxycxy[3]; + + // q = fxy pxy/pz + cxy + // dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1]) + // dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1]) + for(int i=0; ix = v[i].x*pz_recip * fx + cx; + q->y = v[i].y*pz_recip * fy + cy; + + if(dq_dv) + { + dq_dv[2*i + 0].x = fx * pz_recip; + dq_dv[2*i + 0].y = 0; + dq_dv[2*i + 0].z = -fx*v[i].x*pz_recip*pz_recip; + + dq_dv[2*i + 1].x = 0; + dq_dv[2*i + 1].y = fy * pz_recip; + dq_dv[2*i + 1].z = -fy*v[i].y*pz_recip*pz_recip; + } + } +} + +// Compute a pinhole unprojection using a constant fxy, cxy +void mrcal_unproject_pinhole( // output + mrcal_point3_t* v, + mrcal_point2_t* dv_dq, // May be NULL. Each point + // gets a block of 3 + // mrcal_point2_t objects + + // input + const mrcal_point2_t* q, + int N, + const double* fxycxy) +{ + const double fx = fxycxy[0]; + const double fy = fxycxy[1]; + const double cx = fxycxy[2]; + const double cy = fxycxy[3]; + + double fx_recip = 1./fx; + double fy_recip = 1./fy; + for(int i=0; i u = (q-c)/f + // mag(u) = tan(th/2)*2 + // + // So I can compute th. az comes from the direction of u. This is enough to + // compute everything. th is in [0,pi]. + // + // [ sin(th) cos(az) ] [ cos(az) ] + // v = [ sin(th) sin(az) ] ~ [ sin(az) ] + // [ cos(th) ] [ 1/tan(th) ] + // + // mag(u) = tan(th/2)*2 -> mag(u)/2 = tan(th/2) -> + // tan(th) = mag(u) / (1 - (mag(u)/2)^2) + // 1/tan(th) = (1 - 1/4*mag(u)^2) / mag(u) + // + // This has a singularity at u=0 (imager center). But I can scale v to avoid + // this. So + // + // [ cos(az) mag(u) ] + // v = [ sin(az) mag(u) ] + // [ 1 - 1/4*mag(u)^2 ] + // + // I can simplify this even more. az = atan2(u.y,u.x). cos(az) = u.x/mag(u) -> + // + // [ u.x ] + // v = [ u.y ] + // [ 1 - 1/4 mag(u)^2 ] + // + // Test script to confirm that the project/unproject expressions are + // correct. unproj(proj(v))/v should be a constant + // + // import numpy as np + // import numpysane as nps + // f = 2000 + // c = 1000 + // def proj(v): + // m = nps.mag(v) + // scale = 2.0 / (m + v[..., 2]) + // u = v[..., :2] * nps.dummy(scale, -1) + // return u * f + c + // def unproj(q): + // u = (q-c)/f + // muxy = nps.mag(u[..., :2]) + // m = nps.mag(u) + // return nps.mv(nps.cat( u[..., 0], + // u[..., 1], + // 1 - 1./4.* m*m), + // 0, -1) + // v = np.array(((1., 2., 3.), + // (3., -2., -4.))) + // print( unproj(proj(v)) / v) + double fx_recip = 1./fx; + double fy_recip = 1./fy; + for(int i=0; i + // dqx/dv = 1/(1 + (vx/vz)^2) 1/vz^2 ( dvx/dv vz - vx dvz/dv ) = + // = [vz 0 -vx] / (vx^2 + vz^2) + // + // qy ~ arcsin( vy / mag(v) ) -> + // dqy/dv = 1 / sqrt( 1 - vy^2/norm2(v) ) 1/norm2(v) (dvy/dv mag(v) - dmag(v)/dv vy) + // = 1 / sqrt( norm2(v) - vy^2 ) 1/mag(v) ( [0 mag(v) 0] - v/mag(v) vy) + // = 1 / sqrt( norm2(v) - vy^2 ) ( [0 1 0] - v/norm2(v) vy) + // = 1 / sqrt( vx^2 + vz^2 ) ( [0 1 0] - v/norm2(v) vy) + for(int i=0; i vx = vz tan(lon) + // -> 1-sin^2(lat) = vz^2 (1 + tan^2(lon)) = + // cos^2(lat) = (vz/cos(lon))^2 + // + // -> vx = cos(lat) sin(lon) + // vy = sin(lat) + // vz = cos(lat) cos(lon) + // + // mag(v) is arbitrary, and I can simplify: + // + // -> v_unnormalized_x = sin(lon) + // v_unnormalized_y = tan(lat) + // v_unnormalized_z = cos(lon) + // + // If the computational cost of tan(lat) is smaller than of + // sin(lat),cos(lat) and 2 multiplications, then this is a better + // representation. A quick web search tells me that the cost of sincos ~ the + // cost of either sin or cos. And that tan is more costly. So I use the + // normalized form + // + // dv/dlon = [ cos(lat) cos(lon) 0 -cos(lat) sin(lon)] + // dv/dlat = [-sin(lat) sin(lon) cos(lat) -sin(lat) cos(lon)] + double fx_recip = 1./fx; + double fy_recip = 1./fy; + for(int i=0; i u_width_needed/(Nknots-1) = u_interval_size * (1 - Nknots_margin/(Nknots - 1)) + // ---> u_width_needed = u_interval_size * (Nknots - 1 - Nknots_margin) + // ---> u_interval_size = u_width_needed / (Nknots - 1 - Nknots_margin) + int Nknots_margin; + if(config->order == 2) + { + Nknots_margin = 1; + if(config->Nx < 3 || config->Ny < 3) + { + MSG("Quadratic splines: absolute minimum Nx, Ny is 3. Got Nx=%d Ny=%d. Barfing out", + config->Nx, config->Ny); + assert(0); + } + } + else if(config->order == 3) + { + Nknots_margin = 2; + if(config->Nx < 4 || config->Ny < 4) + { + MSG("Cubic splines: absolute minimum Nx, Ny is 4. Got Nx=%d Ny=%d. Barfing out", + config->Nx, config->Ny); + assert(0); + } + } + else + { + MSG("I only support spline order 2 and 3"); + assert(0); + } + + double th_edge_x = (double)config->fov_x_deg/2. * M_PI / 180.; + double u_edge_x = tan(th_edge_x / 2.) * 2; + precomputed->segments_per_u = (config->Nx - 1 - Nknots_margin) / (u_edge_x*2.); +} + +// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper +// only +void _mrcal_precompute_lensmodel_data(mrcal_projection_precomputed_t* precomputed, + const mrcal_lensmodel_t* lensmodel) +{ + // currently only this model has anything + if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + _mrcal_precompute_lensmodel_data_MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC + ( &precomputed->LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed, + &lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config ); + precomputed->ready = true; +} + +bool mrcal_knots_for_splined_models( // buffers must hold at least + // config->Nx and config->Ny values + // respectively + double* ux, double* uy, + const mrcal_lensmodel_t* lensmodel) +{ + if(lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + MSG("This function works only with the MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC model. '%s' passed in", + mrcal_lensmodel_name_unconfigured(lensmodel)); + return false; + } + + mrcal_projection_precomputed_t precomputed_all; + _mrcal_precompute_lensmodel_data(&precomputed_all, lensmodel); + + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config = + &lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config; + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t* precomputed = + &precomputed_all.LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed; + + // The logic I'm reversing is + // double ix = u.x*segments_per_u + (double)(Nx-1)/2.; + for(int i=0; iNx; i++) + ux[i] = + ((double)i - (double)(config->Nx-1)/2.) / + precomputed->segments_per_u; + for(int i=0; iNy; i++) + uy[i] = + ((double)i - (double)(config->Ny-1)/2.) / + precomputed->segments_per_u; + return true; +} + +static int get_Ngradients(const mrcal_lensmodel_t* lensmodel, + int Nintrinsics) +{ + int N = 0; + bool has_core = modelHasCore_fxfycxcy(lensmodel); + bool has_dense_intrinsics_grad = (lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + bool has_sparse_intrinsics_grad = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + int runlen = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) ? + (lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.order + 1) : + 0; + if(has_core) + // qx(fx) and qy(fy) + N += 2; + if(has_dense_intrinsics_grad) + // each of (qx,qy) depends on all the non-core intrinsics + N += 2 * (Nintrinsics-4); + if(has_sparse_intrinsics_grad) + { + // spline coefficients + N += 2*runlen; + } + + return N; +} + +static +void propagate_extrinsics__splined( // output + mrcal_point3_t* dq_deee, + // input + const mrcal_point3_t* dp_deee, + const double* duxy_dp, + const double* ddeltau_dux, + const double* ddeltau_duy, + const double fx, + const double fy) +{ + mrcal_point3_t du_deee[2]; + mul_genN3_gen33_vout(2, (double*)duxy_dp, (double*)dp_deee, (double*)du_deee); + + for(int i=0; i<3; i++) + { + dq_deee[0].xyz[i] = + fx * + ( du_deee[0].xyz[i] * (1. + ddeltau_dux[0]) + + ddeltau_duy[0] * du_deee[1].xyz[i]); + dq_deee[1].xyz[i] = + fy * + ( du_deee[1].xyz[i] * (1. + ddeltau_duy[1]) + + ddeltau_dux[1] * du_deee[0].xyz[i]); + } +} +static +void propagate_extrinsics_cam0__splined( // output + mrcal_point3_t* dq_deee, + // input + const double* dux_dp, + const double* duy_dp, + const double* ddeltau_dux, + const double* ddeltau_duy, + const double fx, + const double fy) +{ + for(int i=0; i<3; i++) + { + dq_deee[0].xyz[i] = + fx * + ( dux_dp[i] * (1. + ddeltau_dux[0]) + + ddeltau_duy[0] * duy_dp[i]); + dq_deee[1].xyz[i] = + fy * + ( duy_dp[i] * (1. + ddeltau_duy[1]) + + ddeltau_dux[1] * dux_dp[i]); + } +} +static +void _project_point_splined( // outputs + mrcal_point2_t* q, + mrcal_point2_t* dq_dfxy, + + double* grad_ABCDx_ABCDy, + int* ivar0, + + // Gradient outputs. May be NULL + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + + // inputs + const mrcal_point3_t* restrict p, + const mrcal_point3_t* restrict dp_drc, + const mrcal_point3_t* restrict dp_dtc, + const mrcal_point3_t* restrict dp_drf, + const mrcal_point3_t* restrict dp_dtf, + + const double* restrict intrinsics, + bool camera_at_identity, + int spline_order, + uint16_t Nx, uint16_t Ny, + double segments_per_u) +{ + // projections out-of-bounds will yield SOME value (function remains + // continuous as we move out-of-bounds), but it wont be particularly + // meaningful + + + // stereographic projection: + // (from https://en.wikipedia.org/wiki/Fisheye_lens) + // u = xy_unit * tan(th/2) * 2 + // + // I compute the normalized (focal-length = 1) projection, and + // use that to look-up deltau + + // th is the angle between the observation and the projection + // center + // + // sin(th) = mag_pxy/mag_p + // cos(th) = z/mag_p + // tan(th/2) = sin(th) / (1 + cos(th)) + + // tan(th/2) = mag_pxy/mag_p / (1 + z/mag_p) = + // = mag_pxy / (mag_p + z) + // u = xy_unit * tan(th/2) * 2 = + // = xy/mag_pxy * mag_pxy/(mag_p + z) * 2 = + // = xy / (mag_p + z) * 2 + // + // The stereographic projection is used to query the spline surface, and it + // is also the projection baseline. I do + // + // q = (u + deltau(u)) * f + c + // + // If the spline surface is at 0 (deltau == 0) then this is a pure + // stereographic projection + double mag_p = sqrt( p->x*p->x + + p->y*p->y + + p->z*p->z ); + double scale = 2.0 / (mag_p + p->z); + + mrcal_point2_t u = {.x = p->x * scale, + .y = p->y * scale}; + // du/dp = d/dp ( xy * scale ) + // = pxy dscale/dp + [I; 0] scale + // dscale/dp = d (2.0 / (mag_p + p->z))/dp = + // = -2/()^2 ( [0,0,1] + dmag/dp) + // = -2/()^2 ( [0,0,1] + 2pt/2mag) + // = -2 scale^2/4 ( [0,0,1] + pt/mag) + // = -scale^2/2 * ( [0,0,1] + pt/mag ) + // = A*[0,0,1] + B*pt + double A = -scale*scale / 2.; + double B = A / mag_p; + double du_dp[2][3] = { { p->x * (B * p->x) + scale, + p->x * (B * p->y), + p->x * (B * p->z + A) }, + { p->y * (B * p->x), + p->y * (B * p->y) + scale, + p->y * (B * p->z + A) } }; + + double ix = u.x*segments_per_u + (double)(Nx-1)/2.; + double iy = u.y*segments_per_u + (double)(Ny-1)/2.; + + mrcal_point2_t deltau; + double ddeltau_dux[2]; + double ddeltau_duy[2]; + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + + if( spline_order == 3 ) + { + int ix0 = (int)ix; + int iy0 = (int)iy; + + // If out-of-bounds, clamp to the nearest valid spline segment. The + // projection will fly off to infinity quickly (we're extrapolating a + // polynomial), but at least it'll stay continuous + if( ix0 < 1) ix0 = 1; + else if(ix0 > Nx-3) ix0 = Nx-3; + if( iy0 < 1) iy0 = 1; + else if(iy0 > Ny-3) iy0 = Ny-3; + + *ivar0 = + 4 + // skip the core + 2*( (iy0-1)*Nx + + (ix0-1) ); + + sample_bspline_surface_cubic(deltau.xy, + ddeltau_dux, ddeltau_duy, + grad_ABCDx_ABCDy, + ix - ix0, iy - iy0, + + // control points + &intrinsics[*ivar0], + 2*Nx); + } + else if( spline_order == 2 ) + { + int ix0 = (int)(ix + 0.5); + int iy0 = (int)(iy + 0.5); + + // If out-of-bounds, clamp to the nearest valid spline segment. The + // projection will fly off to infinity quickly (we're extrapolating a + // polynomial), but at least it'll stay continuous + if( ix0 < 1) ix0 = 1; + else if(ix0 > Nx-2) ix0 = Nx-2; + if( iy0 < 1) iy0 = 1; + else if(iy0 > Ny-2) iy0 = Ny-2; + + *ivar0 = + 4 + // skip the core + 2*( (iy0-1)*Nx + + (ix0-1) ); + + sample_bspline_surface_quadratic(deltau.xy, + ddeltau_dux, ddeltau_duy, + grad_ABCDx_ABCDy, + ix - ix0, iy - iy0, + + // control points + &intrinsics[*ivar0], + 2*Nx); + } + else + { + MSG("I only support spline order==2 or 3. Somehow got %d. This is a bug. Barfing", + spline_order); + assert(0); + } + + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Extrinsics: + // dqx/deee = fx (dux/deee + ddeltaux/du du/deee) + // = fx ( dux/deee (1 + ddeltaux/dux) + ddeltaux/duy duy/deee) + // dqy/deee = fy ( duy/deee (1 + ddeltauy/duy) + ddeltauy/dux dux/deee) + q->x = (u.x + deltau.x) * fx + cx; + q->y = (u.y + deltau.y) * fy + cy; + + if( dq_dfxy ) + { + // I have the projection, and I now need to propagate the gradients + // xy = fxy * distort(xy)/distort(z) + cxy + dq_dfxy->x = u.x + deltau.x; + dq_dfxy->y = u.y + deltau.y; + } + + // convert ddeltau_dixy to ddeltau_duxy + for(int i=0; i<2; i++) + { + ddeltau_dux[i] *= segments_per_u; + ddeltau_duy[i] *= segments_per_u; + } + + if(camera_at_identity) + { + if( dq_drcamera != NULL ) memset(dq_drcamera->xyz, 0, 6*sizeof(double)); + if( dq_dtcamera != NULL ) memset(dq_dtcamera->xyz, 0, 6*sizeof(double)); + if( dq_drframe != NULL ) + propagate_extrinsics__splined( dq_drframe, dp_drf, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + if( dq_dtframe != NULL ) + propagate_extrinsics_cam0__splined( dq_dtframe, + du_dp[0], du_dp[1], + ddeltau_dux, ddeltau_duy, + fx, fy); + } + else + { + if( dq_drcamera != NULL ) + propagate_extrinsics__splined( dq_drcamera, dp_drc, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + if( dq_dtcamera != NULL ) + propagate_extrinsics__splined( dq_dtcamera, dp_dtc, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + if( dq_drframe != NULL ) + propagate_extrinsics__splined( dq_drframe, dp_drf, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + if( dq_dtframe != NULL ) + propagate_extrinsics__splined( dq_dtframe, dp_dtf, + (const double*)du_dp, + ddeltau_dux, ddeltau_duy, + fx,fy); + } +} + +typedef struct +{ + double* pool; + uint16_t run_side_length; + uint16_t ivar_stridey; +} gradient_sparse_meta_t; + + +// This is internal to project() +static +void _propagate_extrinsics_one(mrcal_point3_t* dp_dparam, + const mrcal_point3_t* pt_ref, + const double* drj_dparam, + const double* dtj_dparam, + const double* d_Rj_rj) +{ + // dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0] + // dRj[row0]/drc = dRj[row0]/drj * drj_drc + for(int i=0; i<3; i++) + { + mul_vec3_gen33_vout( pt_ref->xyz, &d_Rj_rj[9*i], dp_dparam[i].xyz ); + mul_vec3_gen33 ( dp_dparam[i].xyz, drj_dparam); + add_vec(3, dp_dparam[i].xyz, &dtj_dparam[3*i] ); + } +} +static +void _propagate_extrinsics_one_rzero(mrcal_point3_t* dp_dparam, + const mrcal_point3_t* pt_ref, + const double* dtj_dparam, + const double* d_Rj_rj) +{ + // dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0] + // dRj[row0]/drc = dRj[row0]/drj * drj_drc + memcpy(dp_dparam->xyz, dtj_dparam, 9*sizeof(double)); +} +static +void _propagate_extrinsics_one_tzero(mrcal_point3_t* dp_dparam, + const mrcal_point3_t* pt_ref, + const double* drj_dparam, + const double* d_Rj_rj) +{ + // dRj[row0]/drj is 3x3 matrix at &d_Rj_rj[0] + // dRj[row0]/drc = dRj[row0]/drj * drj_drc + for(int i=0; i<3; i++) + { + mul_vec3_gen33_vout( pt_ref->xyz, &d_Rj_rj[9*i], dp_dparam[i].xyz ); + mul_vec3_gen33 ( dp_dparam[i].xyz, drj_dparam); + } +} +static +void _propagate_extrinsics_one_rzero_tidentity(mrcal_point3_t* dp_dparam, + const mrcal_point3_t* pt_ref, + const double* d_Rj_rj) +{ + dp_dparam[0] = (mrcal_point3_t){.x = 1.0}; + dp_dparam[1] = (mrcal_point3_t){.y = 1.0}; + dp_dparam[2] = (mrcal_point3_t){.z = 1.0}; +} + +static +void _propagate_extrinsics_one_cam0(mrcal_point3_t* dp_rf, + const mrcal_point3_t* pt_ref, + const double* _d_Rf_rf) +{ + // dRj[row0]/drj is 3x3 matrix at &_d_Rf_rf[0] + // dRj[row0]/drc = dRj[row0]/drj * drj_drc + for(int i=0; i<3; i++) + mul_vec3_gen33_vout( pt_ref->xyz, &_d_Rf_rf[9*i], dp_rf[i].xyz ); +} +static +mrcal_point3_t _propagate_extrinsics( // output + mrcal_point3_t* _dp_drc, + mrcal_point3_t* _dp_dtc, + mrcal_point3_t* _dp_drf, + mrcal_point3_t* _dp_dtf, + mrcal_point3_t** dp_drc, + mrcal_point3_t** dp_dtc, + mrcal_point3_t** dp_drf, + mrcal_point3_t** dp_dtf, + + // input + const mrcal_point3_t* pt_ref, + const geometric_gradients_t* gg, + const double* Rj, const double* d_Rj_rj, + const double* _tj ) +{ + // Rj * pt + tj -> pt + mrcal_point3_t p; + mul_vec3_gen33t_vout(pt_ref->xyz, Rj, p.xyz); + add_vec(3, p.xyz, _tj); + + if(gg != NULL) + { + _propagate_extrinsics_one( _dp_drc, pt_ref, gg->_d_rj_rc, gg->_d_tj_rc, d_Rj_rj); + _propagate_extrinsics_one_rzero_tidentity(_dp_dtc, pt_ref, d_Rj_rj); + _propagate_extrinsics_one_tzero( _dp_drf, pt_ref, gg->_d_rj_rf, d_Rj_rj); + _propagate_extrinsics_one_rzero( _dp_dtf, pt_ref, gg->_d_tj_tf, d_Rj_rj); + *dp_drc = _dp_drc; + *dp_dtc = _dp_dtc; + *dp_drf = _dp_drf; + *dp_dtf = _dp_dtf; + } + else + { + // camera is at the reference. The "joint" coord system is the "frame" + // coord system + // + // p_cam = Rf p_ref + tf + // + // dp/drc = 0 + // dp/dtc = 0 + // dp/drf = reshape(dRf_drf p_ref) + // dp/dtf = I + _propagate_extrinsics_one_cam0(_dp_drf, pt_ref, d_Rj_rj); + + *dp_drc = NULL; + *dp_dtc = NULL; + *dp_drf = _dp_drf; + *dp_dtf = NULL; // this is I. The user of this MUST know to interpret + // it that way + } + return p; +} +// This is internal to project() +static +void _project_point( // outputs + mrcal_point2_t* q, + mrcal_point2_t* p_dq_dfxy, + double* p_dq_dintrinsics_nocore, + double* gradient_sparse_meta_pool, + int runlen, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + mrcal_calobject_warp_t* restrict dq_dcalobject_warp, + int* restrict * dq_dintrinsics_pool_int, + // inputs + const mrcal_point3_t* p, + const double* restrict intrinsics, + const mrcal_lensmodel_t* lensmodel, + const mrcal_calobject_warp_t* dpt_refz_dwarp, + // if NULL then the camera is at the reference + bool camera_at_identity, + const double* Rj, + const int Nintrinsics, + const mrcal_projection_precomputed_t* precomputed, + const mrcal_point3_t* dp_drc, + const mrcal_point3_t* dp_dtc, + const mrcal_point3_t* dp_drf, + const mrcal_point3_t* dp_dtf) +{ + if(lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + // only need 3+3 for quadratic splines + double grad_ABCDx_ABCDy[4+4]; + int ivar0; + + _project_point_splined( // outputs + q, p_dq_dfxy, + grad_ABCDx_ABCDy, + &ivar0, + + dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe, + // inputs + p, + dp_drc, dp_dtc, dp_drf, dp_dtf, + intrinsics, + camera_at_identity, + lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.order, + lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx, + lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny, + precomputed->LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed.segments_per_u); + // WARNING: if I could assume that dq_dintrinsics_pool_double!=NULL then I wouldnt need to copy the context + if(*dq_dintrinsics_pool_int != NULL) + { + *((*dq_dintrinsics_pool_int)++) = ivar0; + memcpy(gradient_sparse_meta_pool, + grad_ABCDx_ABCDy, + sizeof(double)*runlen*2); + } + } + else if(lensmodel->type == MRCAL_LENSMODEL_CAHVOR) + { + project_cahvor( // outputs + q,p_dq_dfxy, + p_dq_dintrinsics_nocore, + dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe, + // inputs + p, + dp_drc, dp_dtc, dp_drf, dp_dtf, + intrinsics, + camera_at_identity, + lensmodel); + } + else if(lensmodel->type == MRCAL_LENSMODEL_CAHVORE) + { + if(!project_cahvore( // outputs + q,p_dq_dfxy, + p_dq_dintrinsics_nocore, + dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe, + // inputs + p, + dp_drc, dp_dtc, dp_drf, dp_dtf, + intrinsics, + camera_at_identity, + lensmodel)) + { + MSG("CAHVORE PROJECTION OF (%f,%f,%f) FAILED. I don't know what to do. Setting result and all gradients to 0", + p->x, p->y, p->z); + memset(q, 0, sizeof(*q)); + if(p_dq_dfxy) memset(p_dq_dfxy, 0, sizeof(*p_dq_dfxy)); + if(p_dq_dintrinsics_nocore) memset(p_dq_dintrinsics_nocore, 0, sizeof(*p_dq_dintrinsics_nocore) * 2 * (Nintrinsics-4)); + if(dq_drcamera) memset(dq_drcamera, 0, sizeof(*dq_drcamera)); + if(dq_dtcamera) memset(dq_dtcamera, 0, sizeof(*dq_dtcamera)); + if(dq_drframe) memset(dq_drframe, 0, sizeof(*dq_drframe)); + if(dq_dtframe) memset(dq_dtframe, 0, sizeof(*dq_dtframe)); + + } + } + else + { + _project_point_parametric( // outputs + q,p_dq_dfxy, + p_dq_dintrinsics_nocore, + dq_drcamera,dq_dtcamera,dq_drframe,dq_dtframe, + // inputs + p, + dp_drc, dp_dtc, dp_drf, dp_dtf, + intrinsics, + camera_at_identity, + lensmodel); + } + + if( dq_dcalobject_warp != NULL && dpt_refz_dwarp != NULL ) + { + // p = proj(Rc Rf warp(x) + Rc tf + tc); + // dp/dw = dp/dRcRf(warp(x)) dR(warp(x))/dwarp(x) dwarp/dw = + // = dp/dtc RcRf dwarp/dw + // dp/dtc is dq_dtcamera + // R is rodrigues(rj) + // dwarp/dw = [0 0 0 ...] + // [0 0 0 ...] + // [a b c ...] + // Let R = [r0 r1 r2] + // dp/dw = dp/dt [a r2 b r2] = + // [a dp/dt r2 b dp/dt r2 ...] + mrcal_point3_t* p_dq_dt; + if(!camera_at_identity) p_dq_dt = dq_dtcamera; + else p_dq_dt = dq_dtframe; + double d[] = + { p_dq_dt[0].xyz[0] * Rj[0*3 + 2] + + p_dq_dt[0].xyz[1] * Rj[1*3 + 2] + + p_dq_dt[0].xyz[2] * Rj[2*3 + 2], + p_dq_dt[1].xyz[0] * Rj[0*3 + 2] + + p_dq_dt[1].xyz[1] * Rj[1*3 + 2] + + p_dq_dt[1].xyz[2] * Rj[2*3 + 2]}; + + for(int i=0; ivalues[i]; + dq_dcalobject_warp[1].values[i] = d[1]*dpt_refz_dwarp->values[i]; + } + } +} + +// Projects 3D point(s), and reports the projection, and all the gradients. This +// is the main internal callback in the optimizer. This operates in one of two modes: +// +// if(calibration_object_width_n == 0) then we're projecting ONE point. In world +// coords this point is at frame_rt->t. frame_rt->r is not referenced. q and the +// gradients reference 2 values (x,y in the imager) +// +// if(calibration_object_width_n > 0) then we're projecting a whole calibration +// object. The pose of this object is given in frame_rt. We project ALL +// calibration_object_width_n*calibration_object_height_n points. q and the +// gradients reference ALL of these points +static +void project( // out + mrcal_point2_t* restrict q, + + // The intrinsics gradients. These are split among several arrays. + // High-parameter-count lens models can return their gradients + // sparsely. All the actual gradient values live in + // dq_dintrinsics_pool_double, a buffer supplied by the caller. If + // dq_dintrinsics_pool_double is not NULL, the rest of the + // variables are assumed non-NULL, and we compute all the + // intrinsics gradients. Conversely, if dq_dintrinsics_pool_double + // is NULL, no intrinsics gradients are computed + double* restrict dq_dintrinsics_pool_double, + int* restrict dq_dintrinsics_pool_int, + double** restrict dq_dfxy, + double** restrict dq_dintrinsics_nocore, + gradient_sparse_meta_t* gradient_sparse_meta, + mrcal_point3_t* restrict dq_drcamera, + mrcal_point3_t* restrict dq_dtcamera, + mrcal_point3_t* restrict dq_drframe, + mrcal_point3_t* restrict dq_dtframe, + mrcal_calobject_warp_t* restrict dq_dcalobject_warp, + + // in + + // everything; includes the core, if there is one + const double* restrict intrinsics, + const mrcal_pose_t* restrict camera_rt, + const mrcal_pose_t* restrict frame_rt, + const mrcal_calobject_warp_t* restrict calobject_warp, + + bool camera_at_identity, // if true, camera_rt is unused + const mrcal_lensmodel_t* lensmodel, + const mrcal_projection_precomputed_t* precomputed, + + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n) +{ + assert(precomputed->ready); + + // Parametric and non-parametric models do different things: + // + // parametric models: + // u = distort(p, distortions) + // q = uxy/uz * fxy + cxy + // + // extrinsic gradients: + // dqx/deee = d( ux/uz * fx + cx)/deee = + // = fx d(ux/uz)/deee = + // = fx/uz^2 ( uz dux/deee - duz/deee ux ) + // + // nonparametric (splined) models + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Extrinsics: + // dqx/deee = fx (dux/deee + ddeltaux/du du/deee) + // = fx ( dux/deee (1 + ddeltaux/dux) + ddeltaux/duy duy/deee) + // dqy/deee = fy ( duy/deee (1 + ddeltauy/duy) + ddeltauy/dux dux/deee) + // + // Intrinsics: + // dq/diii = f ddeltau/diii + // + // So the two kinds of models have completely different expressions for + // their gradients, and I implement them separately + + const int Npoints = + calibration_object_width_n ? + calibration_object_width_n*calibration_object_height_n : 1; + const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + + // I need to compose two transformations + // + // (object in reference frame) -> [frame transform] -> (object in the reference frame) -> + // -> [camera rt] -> (camera frame) + // + // Note that here the frame transform transforms TO the reference frame and + // the camera transform transforms FROM the reference frame. This is how my + // data is expected to be set up + // + // [Rc tc] [Rf tf] = [Rc*Rf Rc*tf + tc] + // [0 1 ] [0 1 ] [0 1 ] + // + // This transformation (and its gradients) is handled by mrcal_compose_rt() + // I refer to the camera*frame transform as the "joint" transform, or the + // letter j + geometric_gradients_t gg; + + double _joint_rt[6]; + double* joint_rt; + + // The caller has an odd-looking array reference [-3]. This is intended, but + // the compiler throws a warning. I silence it here. gcc-10 produces a very + // strange-looking warning that was reported to the maintainers: + // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=97261 +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" + mrcal_pose_t frame_rt_validr = {.t = frame_rt->t}; +#pragma GCC diagnostic pop + if(calibration_object_width_n) frame_rt_validr.r = frame_rt->r; + + if(!camera_at_identity) + { + // make sure I can pass mrcal_pose_t.r as an rt[] transformation + mrcal_compose_rt( _joint_rt, + gg._d_rj_rc, gg._d_rj_rf, + gg._d_tj_rc, gg._d_tj_tf, + camera_rt ->r.xyz, + frame_rt_validr.r.xyz); + joint_rt = _joint_rt; + } + else + { + // We're looking at the reference frame, so this camera transform is + // fixed at the identity. We don't need to compose anything, nor + // propagate gradients for the camera extrinsics, since those don't + // exist in the parameter vector + + // Here the "joint" transform is just the "frame" transform + joint_rt = frame_rt_validr.r.xyz; + } + + // Not using OpenCV distortions, the distortion and projection are not + // coupled + double Rj[3*3]; + double d_Rj_rj[9*3]; + + mrcal_R_from_r(Rj, d_Rj_rj, joint_rt); + + mrcal_point2_t* p_dq_dfxy = NULL; + double* p_dq_dintrinsics_nocore = NULL; + bool has_core = modelHasCore_fxfycxcy(lensmodel); + bool has_dense_intrinsics_grad = (lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + bool has_sparse_intrinsics_grad = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + int runlen = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) ? + (lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config.order + 1) : + 0; + + if(dq_dintrinsics_pool_double != NULL) + { + // nothing by default + *dq_dfxy = NULL; + *dq_dintrinsics_nocore = NULL; + gradient_sparse_meta->pool = NULL; + int ivar_pool = 0; + + if(has_core) + { + // Each point produces 2 measurements. Each one depends on ONE fxy + // element. So Npoints*2 of these + *dq_dfxy = &dq_dintrinsics_pool_double[ivar_pool]; + p_dq_dfxy = (mrcal_point2_t*)*dq_dfxy; + ivar_pool += Npoints*2; + } + if(has_dense_intrinsics_grad) + { + *dq_dintrinsics_nocore = p_dq_dintrinsics_nocore = &dq_dintrinsics_pool_double[ivar_pool]; + ivar_pool += Npoints*2 * (Nintrinsics-4); + } + if(has_sparse_intrinsics_grad) + { + if(lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC) + { + MSG("Unhandled lens model: %d (%s)", + lensmodel->type, + mrcal_lensmodel_name_unconfigured(lensmodel)); + assert(0); + } + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config = + &lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config; + *gradient_sparse_meta = + { + &dq_dintrinsics_pool_double[ivar_pool], + static_cast(config->order+1), + static_cast(2*config->Nx), + }; + ivar_pool += Npoints*2 * runlen; + } + } + + // These are produced by _propagate_extrinsics() and consumed by + // _project_point() + mrcal_point3_t _dp_drc[3]; + mrcal_point3_t _dp_dtc[3]; + mrcal_point3_t _dp_drf[3]; + mrcal_point3_t _dp_dtf[3]; + mrcal_point3_t* dp_drc; + mrcal_point3_t* dp_dtc; + mrcal_point3_t* dp_drf; + mrcal_point3_t* dp_dtf; + + if( calibration_object_width_n == 0 ) + { // projecting discrete points + mrcal_point3_t pt_ref = {}; // Unused + mrcal_point3_t p = + _propagate_extrinsics( _dp_drc,_dp_dtc,_dp_drf,_dp_dtf, + &dp_drc,&dp_dtc,&dp_drf,&dp_dtf, + &pt_ref, + camera_at_identity ? NULL : &gg, + Rj, d_Rj_rj, &joint_rt[3]); + _project_point( q, + p_dq_dfxy, p_dq_dintrinsics_nocore, + gradient_sparse_meta ? gradient_sparse_meta->pool : NULL, + runlen, + dq_drcamera, dq_dtcamera, dq_drframe, dq_dtframe, NULL, + &dq_dintrinsics_pool_int, + + &p, + intrinsics, lensmodel, + NULL, + camera_at_identity, Rj, + Nintrinsics, + precomputed, + dp_drc,dp_dtc,dp_drf,dp_dtf); + } + else + { // projecting a chessboard + int i_pt = 0; + // The calibration object has a simple grid geometry + for(int y = 0; yx2 * dx; + pt_ref.z += calobject_warp->y2 * dy; + dpt_refz_dwarp.x2 = dx; + dpt_refz_dwarp.y2 = dy; + } + + mrcal_point3_t p = + _propagate_extrinsics( _dp_drc,_dp_dtc,_dp_drf,_dp_dtf, + &dp_drc,&dp_dtc,&dp_drf,&dp_dtf, + &pt_ref, + camera_at_identity ? NULL : &gg, + Rj, d_Rj_rj, &joint_rt[3]); + + mrcal_point3_t* dq_drcamera_here = dq_drcamera ? &dq_drcamera [i_pt*2] : NULL; + mrcal_point3_t* dq_dtcamera_here = dq_dtcamera ? &dq_dtcamera [i_pt*2] : NULL; + mrcal_point3_t* dq_drframe_here = dq_drframe ? &dq_drframe [i_pt*2] : NULL; + mrcal_point3_t* dq_dtframe_here = dq_dtframe ? &dq_dtframe [i_pt*2] : NULL; + mrcal_calobject_warp_t* dq_dcalobject_warp_here = dq_dcalobject_warp ? &dq_dcalobject_warp [i_pt*2] : NULL; + + mrcal_point3_t dq_dtcamera_here_dummy[2]; + mrcal_point3_t dq_dtframe_here_dummy [2]; + if(dq_dcalobject_warp) + { + // I need all translation gradients to be available to + // compute the calobject_warp gradients (see the end of the + // _project_point() function above). So I compute those even + // if the caller didn't ask for them + if(!dq_dtcamera_here) dq_dtcamera_here = dq_dtcamera_here_dummy; + if(!dq_dtframe_here) dq_dtframe_here = dq_dtframe_here_dummy; + } + + _project_point(&q[i_pt], + p_dq_dfxy ? &p_dq_dfxy[i_pt] : NULL, + p_dq_dintrinsics_nocore ? &p_dq_dintrinsics_nocore[2*(Nintrinsics-4)*i_pt] : NULL, + gradient_sparse_meta ? &gradient_sparse_meta->pool[i_pt*runlen*2] : NULL, + runlen, + dq_drcamera_here, dq_dtcamera_here, dq_drframe_here, dq_dtframe_here, dq_dcalobject_warp_here, + &dq_dintrinsics_pool_int, + &p, + intrinsics, lensmodel, + &dpt_refz_dwarp, + camera_at_identity, Rj, + Nintrinsics, + precomputed, + dp_drc,dp_dtc,dp_drf,dp_dtf); + i_pt++; + } + } +} + +// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper +// only +bool _mrcal_project_internal( // out + mrcal_point2_t* q, + + // Stored as a row-first array of shape (N,2,3). Each + // row lives in a mrcal_point3_t + mrcal_point3_t* dq_dp, + // core, distortions concatenated. Stored as a row-first + // array of shape (N,2,Nintrinsics). This is a DENSE array. + // High-parameter-count lens models have very sparse + // gradients here, and the internal project() function + // returns those sparsely. For now THIS function densifies + // all of these + double* dq_dintrinsics, + + // in + const mrcal_point3_t* p, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics, + + int Nintrinsics, + const mrcal_projection_precomputed_t* precomputed) +{ + if( dq_dintrinsics == NULL ) + { + for(int i=0; i dq_dintrinsics_pool_double(Ngradients); + int dq_dintrinsics_pool_int [1]; + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; // init to pacify compiler warning + + project( &q[i], + + dq_dintrinsics_pool_double.data(), + dq_dintrinsics_pool_int, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + + NULL, NULL, NULL, dq_dp, NULL, + + // in + intrinsics, NULL, &frame, NULL, true, + lensmodel, precomputed, + 0.0, 0,0); + + int Ncore = 0; + if(dq_dfxy != NULL) + { + Ncore = 4; + + // fxy. off-diagonal elements are 0 + dq_dintrinsics[0*Nintrinsics + 0] = dq_dfxy[0]; + dq_dintrinsics[0*Nintrinsics + 1] = 0.0; + dq_dintrinsics[1*Nintrinsics + 0] = 0.0; + dq_dintrinsics[1*Nintrinsics + 1] = dq_dfxy[1]; + + // cxy. Identity + dq_dintrinsics[0*Nintrinsics + 2] = 1.0; + dq_dintrinsics[0*Nintrinsics + 3] = 0.0; + dq_dintrinsics[1*Nintrinsics + 2] = 0.0; + dq_dintrinsics[1*Nintrinsics + 3] = 1.0; + } + if( dq_dintrinsics_nocore != NULL ) + { + for(int i_xy=0; i_xy<2; i_xy++) + memcpy(&dq_dintrinsics[i_xy*Nintrinsics + Ncore], + &dq_dintrinsics_nocore[i_xy*(Nintrinsics-Ncore)], + (Nintrinsics-Ncore)*sizeof(double)); + } + if(gradient_sparse_meta.pool != NULL) + { + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Intrinsics: + // dq/diii = f ddeltau/diii + // + // ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3]) + + const int ivar0 = dq_dintrinsics_pool_int[0]; + const int len = gradient_sparse_meta.run_side_length; + + const double* ABCDx = &gradient_sparse_meta.pool[0]; + const double* ABCDy = &gradient_sparse_meta.pool[len]; + + const int ivar_stridey = gradient_sparse_meta.ivar_stridey; + const double* fxy = &intrinsics[0]; + for(int i_xy=0; i_xy<2; i_xy++) + for(int iy=0; iytype) || + lensmodel->type == MRCAL_LENSMODEL_PINHOLE)) + { + _mrcal_project_internal_opencv( q, NULL,NULL, + p, N, intrinsics, Nintrinsics); + return true; + } + + mrcal_projection_precomputed_t precomputed; + _mrcal_precompute_lensmodel_data(&precomputed, lensmodel); + + return + _mrcal_project_internal(q, dq_dp, dq_dintrinsics, + p, N, lensmodel, intrinsics, + Nintrinsics, &precomputed); +} + + +// Maps a set of distorted 2D imager points q to a 3D vector in camera +// coordinates that produced these pixel observations. The 3D vector is defined +// up-to-length. The returned vectors v are not normalized, and may have any +// length. +// +// This is the "reverse" direction, so an iterative nonlinear optimization is +// performed internally to compute this result. This is much slower than +// mrcal_project. For OpenCV distortions specifically, OpenCV has +// cvUndistortPoints() (and cv2.undistortPoints()), but these are inaccurate: +// https://github.com/opencv/opencv/issues/8811 +bool mrcal_unproject( // out + mrcal_point3_t* out, + + // in + const mrcal_point2_t* q, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics) +{ + + mrcal_lensmodel_metadata_t meta = mrcal_lensmodel_metadata(lensmodel); + if(!meta.has_gradients) + { + MSG("mrcal_unproject(lensmodel='%s') is not yet implemented: we need gradients", + mrcal_lensmodel_name_unconfigured(lensmodel)); + return false; + } + + mrcal_projection_precomputed_t precomputed; + _mrcal_precompute_lensmodel_data(&precomputed, lensmodel); + + return _mrcal_unproject_internal(out, q, N, lensmodel, intrinsics, &precomputed); +} + +typedef struct +{ + const mrcal_lensmodel_t* lensmodel; + // core, distortions concatenated + const double* intrinsics; + const mrcal_projection_precomputed_t* precomputed; + const mrcal_point2_t* q; +} _unproject_callback_cookie_t; +static +void _unproject_callback(const double* u, + double* x, + double* J, + void* _cookie) +{ + _unproject_callback_cookie_t* cookie = (_unproject_callback_cookie_t*)_cookie; + + // u is the constant-fxy-cxy 2D stereographic + // projection of the hypothesis v. I unproject it stereographically, + // and project it using the actual model + mrcal_point2_t dv_du[3]; + mrcal_pose_t frame = {}; + mrcal_unproject_stereographic( &frame.t, dv_du, + (mrcal_point2_t*)u, 1, + cookie->intrinsics ); + + mrcal_point3_t dq_dtframe[2]; + mrcal_point2_t q_hypothesis; + project( &q_hypothesis, + NULL,NULL,NULL,NULL,NULL, + NULL, NULL, NULL, dq_dtframe, + NULL, + + // in + cookie->intrinsics, + NULL, + &frame, + NULL, + true, + cookie->lensmodel, cookie->precomputed, + 0.0, 0,0); + x[0] = q_hypothesis.x - cookie->q->x; + x[1] = q_hypothesis.y - cookie->q->y; + J[0*2 + 0] = + dq_dtframe[0].x*dv_du[0].x + + dq_dtframe[0].y*dv_du[1].x + + dq_dtframe[0].z*dv_du[2].x; + J[0*2 + 1] = + dq_dtframe[0].x*dv_du[0].y + + dq_dtframe[0].y*dv_du[1].y + + dq_dtframe[0].z*dv_du[2].y; + J[1*2 + 0] = + dq_dtframe[1].x*dv_du[0].x + + dq_dtframe[1].y*dv_du[1].x + + dq_dtframe[1].z*dv_du[2].x; + J[1*2 + 1] = + dq_dtframe[1].x*dv_du[0].y + + dq_dtframe[1].y*dv_du[1].y + + dq_dtframe[1].z*dv_du[2].y; +} + +// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper +// only +bool _mrcal_unproject_internal( // out + mrcal_point3_t* out, + + // in + const mrcal_point2_t* q, + int N, + const mrcal_lensmodel_t* lensmodel, + // core, distortions concatenated + const double* intrinsics, + const mrcal_projection_precomputed_t* precomputed) +{ + // easy special-cases + if( lensmodel->type == MRCAL_LENSMODEL_PINHOLE ) + { + mrcal_unproject_pinhole(out, NULL, q, N, intrinsics); + return true; + } + if( lensmodel->type == MRCAL_LENSMODEL_STEREOGRAPHIC ) + { + mrcal_unproject_stereographic(out, NULL, q, N, intrinsics); + return true; + } + if( lensmodel->type == MRCAL_LENSMODEL_LONLAT ) + { + mrcal_unproject_lonlat(out, NULL, q, N, intrinsics); + return true; + } + if( lensmodel->type == MRCAL_LENSMODEL_LATLON ) + { + mrcal_unproject_latlon(out, NULL, q, N, intrinsics); + return true; + } + + if( lensmodel->type == MRCAL_LENSMODEL_CAHVORE ) + { + const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + + for(int i = Nintrinsics-3; ixyz, NULL, + &v, + 1, + intrinsics ); + // MSG("init. out->xyz[]=(%g,%g)", out->x, out->y); + + + dogleg_parameters2_t dogleg_parameters; + dogleg_getDefaultParameters(&dogleg_parameters); + dogleg_parameters.dogleg_debug = 0; + + _unproject_callback_cookie_t cookie = + { .lensmodel = lensmodel, + .intrinsics = intrinsics, + .precomputed = precomputed, + .q = &q[i] }; + double norm2x = + dogleg_optimize_dense2(out->xyz, 2, 2, _unproject_callback, (void*)&cookie, + &dogleg_parameters, + NULL); + //This needs to be precise; if it isn't, I barf. Shouldn't happen + //very often + + static bool already_complained = false; + // MSG("norm2x = %g", norm2x); + if(norm2x/2.0 > 1e-4) + { + if(!already_complained) + { + // MSG("WARNING: I wasn't able to precisely compute some points. norm2x=%f. Returning nan for those. Will complain just once", + // norm2x); + already_complained = true; + } + double nan = strtod("NAN", NULL); + out->xyz[0] = nan; + out->xyz[1] = nan; + } + else + { + // out[0,1] is the stereographic representation of the observation + // vector using idealized fx,fy,cx,cy. This is already the right + // thing if we're reporting in 2d. Otherwise I need to unproject + + // This is the normal no-error path + mrcal_unproject_stereographic((mrcal_point3_t*)out, NULL, + (mrcal_point2_t*)out, 1, + intrinsics); + if(!model_supports_projection_behind_camera(lensmodel) && out->xyz[2] < 0.0) + { + out->xyz[0] *= -1.0; + out->xyz[1] *= -1.0; + out->xyz[2] *= -1.0; + } + } + + // Advance to the next point. Error or not + out++; + } + return true; +} + +// The following functions define/use the layout of the state vector. In general +// I do: +// +// intrinsics_cam0 +// intrinsics_cam1 +// intrinsics_cam2 +// ... +// extrinsics_cam1 +// extrinsics_cam2 +// extrinsics_cam3 +// ... +// frame0 +// frame1 +// frame2 +// .... +// calobject_warp0 +// calobject_warp1 +// ... + +// From real values to unit-scale values. Optimizer sees unit-scale values +static int pack_solver_state_intrinsics( // out + double* b, // subset based on problem_selections + + // in + const double* intrinsics, // ALL variables. Not a subset + const mrcal_lensmodel_t* lensmodel, + mrcal_problem_selections_t problem_selections, + int Ncameras_intrinsics ) +{ + int i_state = 0; + const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + const int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0; + const int Ndistortions = Nintrinsics - Ncore; + + for(int icam_intrinsics=0; icam_intrinsics < Ncameras_intrinsics; icam_intrinsics++) + { + if( problem_selections.do_optimize_intrinsics_core && Ncore ) + { + const mrcal_intrinsics_core_t* intrinsics_core = (const mrcal_intrinsics_core_t*)intrinsics; + b[i_state++] = intrinsics_core->focal_xy [0] / SCALE_INTRINSICS_FOCAL_LENGTH; + b[i_state++] = intrinsics_core->focal_xy [1] / SCALE_INTRINSICS_FOCAL_LENGTH; + b[i_state++] = intrinsics_core->center_xy[0] / SCALE_INTRINSICS_CENTER_PIXEL; + b[i_state++] = intrinsics_core->center_xy[1] / SCALE_INTRINSICS_CENTER_PIXEL; + } + + if( problem_selections.do_optimize_intrinsics_distortions ) + + for(int i = 0; ix2 / SCALE_CALOBJECT_WARP; + b[i_state++] = calobject_warp->y2 / SCALE_CALOBJECT_WARP; + } + + assert(i_state == Nstate_ref); +} + +// Same as above, but packs/unpacks a vector instead of structures +void mrcal_pack_solver_state_vector( // out, in + double* b, // FULL state on input, unitless + // state on output + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + int Npoints_variable = Npoints - Npoints_fixed; + + int i_state = 0; + + i_state += pack_solver_state_intrinsics_subset_to_subset( b, + lensmodel, problem_selections, + Ncameras_intrinsics ); + + if( problem_selections.do_optimize_extrinsics ) + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + { + mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&b[i_state]); + + b[i_state++] = extrinsics_fromref->r.xyz[0] / SCALE_ROTATION_CAMERA; + b[i_state++] = extrinsics_fromref->r.xyz[1] / SCALE_ROTATION_CAMERA; + b[i_state++] = extrinsics_fromref->r.xyz[2] / SCALE_ROTATION_CAMERA; + + b[i_state++] = extrinsics_fromref->t.xyz[0] / SCALE_TRANSLATION_CAMERA; + b[i_state++] = extrinsics_fromref->t.xyz[1] / SCALE_TRANSLATION_CAMERA; + b[i_state++] = extrinsics_fromref->t.xyz[2] / SCALE_TRANSLATION_CAMERA; + } + + if( problem_selections.do_optimize_frames ) + { + for(int iframe = 0; iframe < Nframes; iframe++) + { + mrcal_pose_t* frames_toref = (mrcal_pose_t*)(&b[i_state]); + b[i_state++] = frames_toref->r.xyz[0] / SCALE_ROTATION_FRAME; + b[i_state++] = frames_toref->r.xyz[1] / SCALE_ROTATION_FRAME; + b[i_state++] = frames_toref->r.xyz[2] / SCALE_ROTATION_FRAME; + + b[i_state++] = frames_toref->t.xyz[0] / SCALE_TRANSLATION_FRAME; + b[i_state++] = frames_toref->t.xyz[1] / SCALE_TRANSLATION_FRAME; + b[i_state++] = frames_toref->t.xyz[2] / SCALE_TRANSLATION_FRAME; + } + + for(int i_point = 0; i_point < Npoints_variable; i_point++) + { + mrcal_point3_t* points = (mrcal_point3_t*)(&b[i_state]); + b[i_state++] = points->xyz[0] / SCALE_POSITION_POINT; + b[i_state++] = points->xyz[1] / SCALE_POSITION_POINT; + b[i_state++] = points->xyz[2] / SCALE_POSITION_POINT; + } + } + + if( has_calobject_warp(problem_selections,Nobservations_board) ) + { + mrcal_calobject_warp_t* calobject_warp = (mrcal_calobject_warp_t*)(&b[i_state]); + b[i_state++] = calobject_warp->x2 / SCALE_CALOBJECT_WARP; + b[i_state++] = calobject_warp->y2 / SCALE_CALOBJECT_WARP; + } +} + +static int unpack_solver_state_intrinsics( // out + + // Ncameras_intrinsics of these + double* intrinsics, // ALL variables. Not a subset. + // I don't touch the elemnents I'm + // not optimizing + + // in + const double* b, // subset based on problem_selections + const mrcal_lensmodel_t* lensmodel, + mrcal_problem_selections_t problem_selections, + int intrinsics_stride, + int Ncameras_intrinsics ) +{ + if( !problem_selections.do_optimize_intrinsics_core && + !problem_selections.do_optimize_intrinsics_distortions ) + return 0; + + const int Nintrinsics = mrcal_lensmodel_num_params(lensmodel); + const int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0; + + int i_state = 0; + for(int icam_intrinsics=0; icam_intrinsics < Ncameras_intrinsics; icam_intrinsics++) + { + if( problem_selections.do_optimize_intrinsics_core && Ncore ) + { + intrinsics[icam_intrinsics*intrinsics_stride + 0] = b[i_state++] * SCALE_INTRINSICS_FOCAL_LENGTH; + intrinsics[icam_intrinsics*intrinsics_stride + 1] = b[i_state++] * SCALE_INTRINSICS_FOCAL_LENGTH; + intrinsics[icam_intrinsics*intrinsics_stride + 2] = b[i_state++] * SCALE_INTRINSICS_CENTER_PIXEL; + intrinsics[icam_intrinsics*intrinsics_stride + 3] = b[i_state++] * SCALE_INTRINSICS_CENTER_PIXEL; + } + + if( problem_selections.do_optimize_intrinsics_distortions ) + { + for(int i = 0; ir.xyz[0] = b[i_state++] * SCALE_ROTATION_CAMERA; + extrinsic->r.xyz[1] = b[i_state++] * SCALE_ROTATION_CAMERA; + extrinsic->r.xyz[2] = b[i_state++] * SCALE_ROTATION_CAMERA; + + extrinsic->t.xyz[0] = b[i_state++] * SCALE_TRANSLATION_CAMERA; + extrinsic->t.xyz[1] = b[i_state++] * SCALE_TRANSLATION_CAMERA; + extrinsic->t.xyz[2] = b[i_state++] * SCALE_TRANSLATION_CAMERA; + return i_state; +} + +static int unpack_solver_state_framert_one(// out + mrcal_pose_t* frame, + + // in + const double* b) +{ + int i_state = 0; + frame->r.xyz[0] = b[i_state++] * SCALE_ROTATION_FRAME; + frame->r.xyz[1] = b[i_state++] * SCALE_ROTATION_FRAME; + frame->r.xyz[2] = b[i_state++] * SCALE_ROTATION_FRAME; + + frame->t.xyz[0] = b[i_state++] * SCALE_TRANSLATION_FRAME; + frame->t.xyz[1] = b[i_state++] * SCALE_TRANSLATION_FRAME; + frame->t.xyz[2] = b[i_state++] * SCALE_TRANSLATION_FRAME; + return i_state; + +} + +static int unpack_solver_state_point_one(// out + mrcal_point3_t* point, + + // in + const double* b) +{ + int i_state = 0; + point->xyz[0] = b[i_state++] * SCALE_POSITION_POINT; + point->xyz[1] = b[i_state++] * SCALE_POSITION_POINT; + point->xyz[2] = b[i_state++] * SCALE_POSITION_POINT; + return i_state; +} + +static int unpack_solver_state_calobject_warp(// out + mrcal_calobject_warp_t* calobject_warp, + + // in + const double* b) +{ + int i_state = 0; + calobject_warp->x2 = b[i_state++] * SCALE_CALOBJECT_WARP; + calobject_warp->y2 = b[i_state++] * SCALE_CALOBJECT_WARP; + return i_state; +} + +// From unit-scale values to real values. Optimizer sees unit-scale values +static void unpack_solver_state( // out + + // ALL intrinsics; whether we're optimizing + // them or not. Don't touch the parts of this + // array we're not optimizing + double* intrinsics_all, // Ncameras_intrinsics of these + + mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these + mrcal_pose_t* frames_toref, // Nframes of these + mrcal_point3_t* points, // Npoints of these + mrcal_calobject_warp_t* calobject_warp, // 1 of these + + // in + const double* b, + const mrcal_lensmodel_t* lensmodel, + mrcal_problem_selections_t problem_selections, + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, int Npoints_variable, + int Nobservations_board, + int Nstate_ref) +{ + int i_state = unpack_solver_state_intrinsics(intrinsics_all, + b, lensmodel, problem_selections, + mrcal_lensmodel_num_params(lensmodel), + Ncameras_intrinsics); + + if( problem_selections.do_optimize_extrinsics ) + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &b[i_state] ); + + if( problem_selections.do_optimize_frames ) + { + for(int iframe = 0; iframe < Nframes; iframe++) + i_state += unpack_solver_state_framert_one( &frames_toref[iframe], &b[i_state] ); + for(int i_point = 0; i_point < Npoints_variable; i_point++) + i_state += unpack_solver_state_point_one( &points[i_point], &b[i_state] ); + } + + if( has_calobject_warp(problem_selections,Nobservations_board) ) + i_state += unpack_solver_state_calobject_warp(calobject_warp, &b[i_state]); + + assert(i_state == Nstate_ref); +} +// Same as above, but packs/unpacks a vector instead of structures +void mrcal_unpack_solver_state_vector( // out, in + double* b, // unitless state on input, + // scaled, meaningful state on + // output + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + int Npoints_variable = Npoints - Npoints_fixed; + + int i_state = + unpack_solver_state_intrinsics_subset_to_subset(b, + lensmodel, problem_selections, + Ncameras_intrinsics); + + if( problem_selections.do_optimize_extrinsics ) + { + mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&b[i_state]); + for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++) + i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &b[i_state] ); + } + + if( problem_selections.do_optimize_frames ) + { + mrcal_pose_t* frames_toref = (mrcal_pose_t*)(&b[i_state]); + for(int iframe = 0; iframe < Nframes; iframe++) + i_state += unpack_solver_state_framert_one( &frames_toref[iframe], &b[i_state] ); + mrcal_point3_t* points = (mrcal_point3_t*)(&b[i_state]); + for(int i_point = 0; i_point < Npoints_variable; i_point++) + i_state += unpack_solver_state_point_one( &points[i_point], &b[i_state] ); + } + if( has_calobject_warp(problem_selections,Nobservations_board) ) + { + mrcal_calobject_warp_t* calobject_warp = (mrcal_calobject_warp_t*)(&b[i_state]); + i_state += unpack_solver_state_calobject_warp(calobject_warp, &b[i_state]); + } +} + +int mrcal_state_index_intrinsics(int icam_intrinsics, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(Ncameras_intrinsics <= 0) + return -1; + int Nintrinsics = mrcal_num_intrinsics_optimization_params(problem_selections, lensmodel); + if(Nintrinsics <= 0) + return -1; + if(!(0 <= icam_intrinsics && icam_intrinsics < Ncameras_intrinsics)) + return -1; + return icam_intrinsics * Nintrinsics; +} + +int mrcal_num_states_intrinsics(int Ncameras_intrinsics, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + return + Ncameras_intrinsics * + mrcal_num_intrinsics_optimization_params(problem_selections, lensmodel); +} + +int mrcal_state_index_extrinsics(int icam_extrinsics, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(Ncameras_extrinsics <= 0) + return -1; + if(!problem_selections.do_optimize_extrinsics) + return -1; + if(!(0 <= icam_extrinsics && icam_extrinsics < Ncameras_extrinsics)) + return -1; + + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + (icam_extrinsics*6); +} + +int mrcal_num_states_extrinsics(int Ncameras_extrinsics, + mrcal_problem_selections_t problem_selections) +{ + return problem_selections.do_optimize_extrinsics ? (6*Ncameras_extrinsics) : 0; +} + +int mrcal_state_index_frames(int iframe, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(Nframes <= 0) + return -1; + if(!problem_selections.do_optimize_frames) + return -1; + if(!(0 <= iframe && iframe < Nframes)) + return -1; + + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + mrcal_num_states_extrinsics(Ncameras_extrinsics, + problem_selections) + + (iframe*6); +} + +int mrcal_num_states_frames(int Nframes, + mrcal_problem_selections_t problem_selections) +{ + return problem_selections.do_optimize_frames ? (6*Nframes) : 0; +} + +int mrcal_state_index_points(int i_point, + int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + int Npoints_variable = Npoints - Npoints_fixed; + + if(Npoints_variable <= 0) + return -1; + if(!problem_selections.do_optimize_frames) + return -1; + if(!(0 <= i_point && i_point < Npoints_variable)) + return -1; + + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + mrcal_num_states_extrinsics(Ncameras_extrinsics, + problem_selections) + + mrcal_num_states_frames (Nframes, + problem_selections) + + (i_point*3); +} + +int mrcal_num_states_points(int Npoints, int Npoints_fixed, + mrcal_problem_selections_t problem_selections) +{ + int Npoints_variable = Npoints - Npoints_fixed; + return problem_selections.do_optimize_frames ? (Npoints_variable*3) : 0; +} + +int mrcal_state_index_calobject_warp(int Ncameras_intrinsics, int Ncameras_extrinsics, + int Nframes, + int Npoints, int Npoints_fixed, int Nobservations_board, + mrcal_problem_selections_t problem_selections, + const mrcal_lensmodel_t* lensmodel) +{ + if(!has_calobject_warp(problem_selections,Nobservations_board)) + return -1; + + return + mrcal_num_states_intrinsics(Ncameras_intrinsics, + problem_selections, + lensmodel) + + mrcal_num_states_extrinsics(Ncameras_extrinsics, + problem_selections) + + mrcal_num_states_frames (Nframes, + problem_selections) + + mrcal_num_states_points (Npoints, Npoints_fixed, + problem_selections); +} + +int mrcal_num_states_calobject_warp(mrcal_problem_selections_t problem_selections, + int Nobservations_board) +{ + if(has_calobject_warp(problem_selections,Nobservations_board)) + return MRCAL_NSTATE_CALOBJECT_WARP; + return 0; +} + +// Reports the icam_extrinsics corresponding to a given icam_intrinsics. +// +// If we're solving a vanilla calibration problem (stationary cameras observing +// a moving calibration object), each camera has a unique intrinsics index and a +// unique extrinsics index. This function reports the latter, given the former. +// +// On success, the result is written to *icam_extrinsics, and we return true. If +// the given camera is at the reference coordinate system, it has no extrinsics, +// and we report -1. +// +// If we have moving cameras (NOT a vanilla calibration problem), there isn't a +// single icam_extrinsics for a given icam_intrinsics, and we report an error by +// returning false +static +bool _corresponding_icam_extrinsics__check( const mrcal_camera_index_t* icam, int i, + int* icam_map_to_extrinsics, + int* icam_map_to_intrinsics, + const char* what) +{ + int icam_intrinsics = icam->intrinsics; + int icam_extrinsics = icam->extrinsics; + + if(icam_extrinsics < 0) icam_extrinsics = -1; + + if(icam_map_to_intrinsics[icam_extrinsics+1] == -100) + icam_map_to_intrinsics[icam_extrinsics+1] = icam_intrinsics; + else if(icam_map_to_intrinsics[icam_extrinsics+1] != icam_intrinsics) + { + MSG("Cannot compute icam_extrinsics. I don't have a vanilla calibration problem: %s observation %d has icam_intrinsics,icam_extrinsics %d,%d while I saw %d,%d previously", + what, i, + icam_map_to_intrinsics[icam_extrinsics+1], icam_extrinsics, + icam_intrinsics, icam_extrinsics); + return false; + } + + if(icam_map_to_extrinsics[icam_intrinsics] == -100) + icam_map_to_extrinsics[icam_intrinsics] = icam_extrinsics; + else if(icam_map_to_extrinsics[icam_intrinsics] != icam_extrinsics) + { + MSG("Cannot compute icam_extrinsics. I don't have a vanilla calibration problem: %s observation %d has icam_intrinsics,icam_extrinsics %d,%d while I saw %d,%d previously", + what, i, + icam_intrinsics, icam_map_to_extrinsics[icam_intrinsics], + icam_intrinsics, icam_extrinsics); + return false; + } + return true; +} +bool mrcal_corresponding_icam_extrinsics(// out + int* icam_extrinsics, + + // in + int icam_intrinsics, + int Ncameras_intrinsics, + int Ncameras_extrinsics, + int Nobservations_board, + const mrcal_observation_board_t* observations_board, + int Nobservations_point, + const mrcal_observation_point_t* observations_point) +{ + if( !(Ncameras_intrinsics == Ncameras_extrinsics || + Ncameras_intrinsics == Ncameras_extrinsics+1 ) ) + { + MSG("Cannot compute icam_extrinsics. I don't have a vanilla calibration problem (stationary cameras, cam0 is reference)"); + return false; + } + + std::vector icam_map_to_extrinsics(Ncameras_intrinsics); + std::vector icam_map_to_intrinsics(Ncameras_extrinsics+1); + for(int i=0; i k stdevs past the mean. I make + // a broad assumption that the error distribution is normally-distributed, + // with mean 0. This is reasonable because this function is applied after + // running the optimization. + // + // The threshold is based on 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 + // extra points: all points worse than the lower threshold. This serves to + // reduce the required re-optimizations + + const double k0 = 4.0; + const double k1 = 5.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); + + + // 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; \ + 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++; + + + + + + /////////////// Compute the variance to set the threshold + bool foundNewOutliers = false; + + double var = 0.0; + LOOP_BOARD_OBSERVATION(true) + { + LOOP_BOARD_FEATURE() + { + LOOP_BOARD_FEATURE_HEADER(); + + if(*weight <= 0.0) + { + (*Noutliers_board)++; + continue; + } + + double dx = x_boards[2*i_pt_board + 0]; + double dy = x_boards[2*i_pt_board + 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; +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #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" +#endif + + + // 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(); + } + } + 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)); + + ///////////// Any new outliers found? + LOOP_BOARD_OBSERVATION(!foundNewOutliers) + { + LOOP_BOARD_FEATURE() + { + LOOP_BOARD_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]; + + // I have sigma = sqrt(var). Outliers have abs(x) > k*sigma + // -> x^2 > k^2 var + if(dx*dx > k1*k1*var ) + { + foundNewOutliers = true; + break; + } + } + LOOP_TRIANGULATED_POINT_FOOTER(); + } + } + if(!foundNewOutliers) + 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) + { + int Npt_inlier = 0; + int Npt_outlier = 0; + LOOP_BOARD_FEATURE() + { + LOOP_BOARD_FEATURE_HEADER(); + if(*weight <= 0.0) + { + Npt_outlier++; + continue; + } + Npt_inlier++; + + 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 > k0*k0*var || + dy*dy > k0*k0*var ) + { + *weight *= -1.0; + (*Noutliers_board)++; + } + } + + if(Npt_inlier < 3) + MSG("WARNING: Board observation %d (icam_intrinsics=%d, icam_extrinsics=%d, iframe=%d) had almost all of its points thrown out as outliers: only %d/%d remain. CHOLMOD is about to complain about a non-positive-definite JtJ. Something is wrong with this observation", + i_observation_board, + observation->icam.intrinsics, + observation->icam.extrinsics, + observation->iframe, + 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)++; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: outliers not returned to the caller yet, so I simply print them out here" +#endif + 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 +} + + +typedef struct +{ + // these are all UNPACKED + const double* intrinsics; // Ncameras_intrinsics * NlensParams of these + const mrcal_pose_t* extrinsics_fromref; // Ncameras_extrinsics of these. Transform FROM the reference frame + const mrcal_pose_t* frames_toref; // Nframes of these. Transform TO the reference frame + const mrcal_point3_t* points; // Npoints of these. In the reference frame + const mrcal_calobject_warp_t* calobject_warp; // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + // in + int Ncameras_intrinsics, Ncameras_extrinsics, Nframes; + int Npoints, Npoints_fixed; + + const mrcal_observation_board_t* observations_board; + const mrcal_point3_t* observations_board_pool; + int Nobservations_board; + + const mrcal_observation_point_t* observations_point; + 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; + const mrcal_problem_constants_t* problem_constants; + + double calibration_object_spacing; + int calibration_object_width_n; + int calibration_object_height_n; + + const int Nmeasurements, N_j_nonzero, Nintrinsics; +} callback_context_t; + +static +void penalty_range_normalization(// out + double* penalty, double* dpenalty_ddistsq, + + // in + // SIGNED distance. <0 means "behind the camera" + const double distsq, + const callback_context_t* ctx, + const double weight) +{ + const double maxsq = ctx->problem_constants->point_max_range*ctx->problem_constants->point_max_range; + if(distsq > maxsq) + { + *penalty = weight * (distsq/maxsq - 1.0); + *dpenalty_ddistsq = weight*(1. / maxsq); + return; + } + + const double minsq = ctx->problem_constants->point_min_range*ctx->problem_constants->point_min_range; + if(distsq < minsq) + { + // too close OR behind the camera + *penalty = weight*(1.0 - distsq/minsq); + *dpenalty_ddistsq = weight*(-1. / minsq); + return; + } + + *penalty = *dpenalty_ddistsq = 0.0; +} +static +void optimizer_callback(// input state + const double* packed_state, + + // output measurements + double* x, + + // Jacobian + cholmod_sparse* Jt, + + const callback_context_t* ctx) +{ + double norm2_error = 0.0; + + int iJacobian = 0; + int iMeasurement = 0; + + int* Jrowptr = Jt ? (int*) Jt->p : NULL; + int* Jcolidx = Jt ? (int*) Jt->i : NULL; + double* Jval = Jt ? (double*)Jt->x : NULL; +#define STORE_JACOBIAN(col, g) \ + do \ + { \ + if(Jt) { \ + Jcolidx[ iJacobian ] = col; \ + Jval [ iJacobian ] = g; \ + } \ + iJacobian++; \ + } while(0) +#define STORE_JACOBIAN2(col0, g0, g1) \ + do \ + { \ + if(Jt) { \ + Jcolidx[ iJacobian+0 ] = col0+0; \ + Jval [ iJacobian+0 ] = g0; \ + Jcolidx[ iJacobian+1 ] = col0+1; \ + Jval [ iJacobian+1 ] = g1; \ + } \ + iJacobian += 2; \ + } while(0) +#define STORE_JACOBIAN3(col0, g0, g1, g2) \ + do \ + { \ + if(Jt) { \ + Jcolidx[ iJacobian+0 ] = col0+0; \ + Jval [ iJacobian+0 ] = g0; \ + Jcolidx[ iJacobian+1 ] = col0+1; \ + Jval [ iJacobian+1 ] = g1; \ + Jcolidx[ iJacobian+2 ] = col0+2; \ + Jval [ iJacobian+2 ] = g2; \ + } \ + iJacobian += 3; \ + } while(0) +#define STORE_JACOBIAN_N(col0, g0, scale, N) \ + do \ + { \ + if(Jt) { \ + for(int i=0; ilensmodel) ? 4 : 0; + int Ncore_state = (modelHasCore_fxfycxcy(&ctx->lensmodel) && + ctx->problem_selections.do_optimize_intrinsics_core) ? 4 : 0; + + // If I'm locking down some parameters, then the state vector contains a + // subset of my data. I reconstitute the intrinsics and extrinsics here. + // I do the frame poses later. This is a good way to do it if I have few + // cameras. With many cameras (this will be slow) + + // WARNING: sparsify this. This is potentially a BIG thing on the stack + std::vector> intrinsics_all(ctx->Ncameras_intrinsics, std::vector(ctx->Nintrinsics)); + std::vector camera_rt(ctx->Ncameras_extrinsics); + + mrcal_calobject_warp_t calobject_warp_local = {}; + const int i_var_calobject_warp = + mrcal_state_index_calobject_warp(ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + if(has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board)) + unpack_solver_state_calobject_warp(&calobject_warp_local, &packed_state[i_var_calobject_warp]); + else if(ctx->calobject_warp != NULL) + calobject_warp_local = *ctx->calobject_warp; + + for(int icam_intrinsics=0; + icam_intrinsicsNcameras_intrinsics; + icam_intrinsics++) + { + // Construct the FULL intrinsics vector, based on either the + // optimization vector or the inputs, depending on what we're optimizing + double* intrinsics_here = &intrinsics_all[icam_intrinsics][0]; + + 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(Ncore) + { + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + intrinsics_here[0] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_FOCAL_LENGTH; + intrinsics_here[1] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_FOCAL_LENGTH; + intrinsics_here[2] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_CENTER_PIXEL; + intrinsics_here[3] = packed_state[i_var_intrinsics++] * SCALE_INTRINSICS_CENTER_PIXEL; + } + else + memcpy( intrinsics_here, + &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics], + Ncore*sizeof(double) ); + } + int nDistortion = ctx->Nintrinsics-Ncore; + if (nDistortion) { + double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore]; + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + for(int i = 0; iNintrinsics-Ncore; i++) + distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION; + } + else { + memcpy( distortions_here, + &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore], + (ctx->Nintrinsics-Ncore)*sizeof(double) ); + } + } + } + for(int icam_extrinsics=0; + icam_extrinsicsNcameras_extrinsics; + icam_extrinsics++) + { + if( icam_extrinsics < 0 ) continue; + + const int i_var_camera_rt = + mrcal_state_index_extrinsics(icam_extrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + if(ctx->problem_selections.do_optimize_extrinsics) + unpack_solver_state_extrinsics_one(&camera_rt[icam_extrinsics], &packed_state[i_var_camera_rt]); + else + memcpy(&camera_rt[icam_extrinsics], &ctx->extrinsics_fromref[icam_extrinsics], sizeof(mrcal_pose_t)); + } + + int i_feature = 0; + for(int i_observation_board = 0; + i_observation_board < ctx->Nobservations_board; + i_observation_board++) + { + const mrcal_observation_board_t* observation = &ctx->observations_board[i_observation_board]; + + const int icam_intrinsics = observation->icam.intrinsics; + const int icam_extrinsics = observation->icam.extrinsics; + const int iframe = observation->iframe; + + + // Some of these are bogus if problem_selections says they're inactive + const int i_var_frame_rt = + mrcal_state_index_frames(iframe, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + mrcal_pose_t frame_rt; + if(ctx->problem_selections.do_optimize_frames) + unpack_solver_state_framert_one(&frame_rt, &packed_state[i_var_frame_rt]); + else + memcpy(&frame_rt, &ctx->frames_toref[iframe], sizeof(mrcal_pose_t)); + + const int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + // invalid if icam_extrinsics < 0, but unused in that case + const int i_var_camera_rt = + mrcal_state_index_extrinsics(icam_extrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + // these are computed in respect to the real-unit parameters, + // NOT the unit-scale parameters used by the optimizer + std::vector dq_drcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_dtcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_drframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_dtframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector dq_dcalobject_warp(ctx->calibration_object_width_n*ctx->calibration_object_height_n*2); + std::vector q_hypothesis (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + // I get the intrinsics gradients in separate arrays, possibly sparsely. + // All the data lives in dq_dintrinsics_pool_double[], with the other data + // indicating the meaning of the values in the pool. + // + // dq_dfxy serves a special-case for a perspective core. Such models + // are very common, and they have x = fx vx/vz + cx and y = fy vy/vz + + // cy. So x depends on fx and NOT on fy, and similarly for y. Similar + // for cx,cy, except we know the gradient value beforehand. I support + // this case explicitly here. I store dx/dfx and dy/dfy; no cross terms + int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics); + + std::vector dq_dintrinsics_pool_double(ctx->calibration_object_width_n*ctx->calibration_object_height_n*Ngradients); + std::vector dq_dintrinsics_pool_int (ctx->calibration_object_width_n*ctx->calibration_object_height_n); + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; + + int splined_intrinsics_grad_irun = 0; + + bool camera_at_identity = icam_extrinsics < 0; + project(q_hypothesis.data(), + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_double.data() : NULL, + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_int.data() : NULL, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + + ctx->problem_selections.do_optimize_extrinsics ? + dq_drcamera.data() : NULL, + ctx->problem_selections.do_optimize_extrinsics ? + dq_dtcamera.data() : NULL, + ctx->problem_selections.do_optimize_frames ? + dq_drframe.data() : NULL, + ctx->problem_selections.do_optimize_frames ? + dq_dtframe.data() : NULL, + has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ? + dq_dcalobject_warp.data() : NULL, + + // input + intrinsics_all[icam_intrinsics].data(), + camera_at_identity ? NULL : &camera_rt[icam_extrinsics], &frame_rt, + ctx->calobject_warp == NULL ? NULL : &calobject_warp_local, + camera_at_identity, + &ctx->lensmodel, &ctx->precomputed, + ctx->calibration_object_spacing, + ctx->calibration_object_width_n, + ctx->calibration_object_height_n); + + for(int i_pt=0; + i_pt < ctx->calibration_object_width_n*ctx->calibration_object_height_n; + i_pt++, i_feature++) + { + const mrcal_point3_t* qx_qy_w__observed = &ctx->observations_board_pool[i_feature]; + double weight = qx_qy_w__observed->z; + + if(weight >= 0.0) + { + // I have my two measurements (dx, dy). I propagate their + // gradient and store them + for( int i_xy=0; i_xy<2; i_xy++ ) + { + const double err = (q_hypothesis[i_pt].xy[i_xy] - qx_qy_w__observed->xyz[i_xy]) * weight; + + if(ctx->verbose) + MSG("obs/frame/cam_i/cam_e/dot: %d %d %d %d %d err: %g", + i_observation_board, iframe, icam_intrinsics, icam_extrinsics, i_pt, err); + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = err; + norm2_error += err*err; + + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + // fx,fy. x depends on fx only. y depends on fy only + STORE_JACOBIAN( i_var_intrinsics + i_xy, + dq_dfxy[i_pt*2 + i_xy] * + weight * SCALE_INTRINSICS_FOCAL_LENGTH ); + + // cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only + STORE_JACOBIAN( i_var_intrinsics + i_xy+2, + weight * SCALE_INTRINSICS_CENTER_PIXEL ); + } + + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + if(gradient_sparse_meta.pool != NULL) + { + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Intrinsics: + // dq/diii = f ddeltau/diii + // + // ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3]) + const int ivar0 = dq_dintrinsics_pool_int[splined_intrinsics_grad_irun] - + ( ctx->problem_selections.do_optimize_intrinsics_core ? 0 : 4 ); + + const int len = gradient_sparse_meta.run_side_length; + const double* ABCDx = &gradient_sparse_meta.pool[len*2*splined_intrinsics_grad_irun + 0]; + const double* ABCDy = &gradient_sparse_meta.pool[len*2*splined_intrinsics_grad_irun + len]; + + const int ivar_stridey = gradient_sparse_meta.ivar_stridey; + const double* fxy = &intrinsics_all[icam_intrinsics][0]; + + for(int iy=0; iyNintrinsics-Ncore; i++) + STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, + dq_dintrinsics_nocore[i_pt*2*(ctx->Nintrinsics-Ncore) + + i_xy*(ctx->Nintrinsics-Ncore) + + i] * + weight * SCALE_DISTORTION ); + } + } + + if( ctx->problem_selections.do_optimize_extrinsics ) + if( icam_extrinsics >= 0 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, + dq_drcamera[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_ROTATION_CAMERA); + STORE_JACOBIAN3( i_var_camera_rt + 3, + dq_dtcamera[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_TRANSLATION_CAMERA); + } + + if( ctx->problem_selections.do_optimize_frames ) + { + STORE_JACOBIAN3( i_var_frame_rt + 0, + dq_drframe[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_ROTATION_FRAME, + dq_drframe[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_ROTATION_FRAME, + dq_drframe[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_ROTATION_FRAME); + STORE_JACOBIAN3( i_var_frame_rt + 3, + dq_dtframe[i_pt * 2 + i_xy].xyz[0] * + weight * SCALE_TRANSLATION_FRAME, + dq_dtframe[i_pt * 2 + i_xy].xyz[1] * + weight * SCALE_TRANSLATION_FRAME, + dq_dtframe[i_pt * 2 + i_xy].xyz[2] * + weight * SCALE_TRANSLATION_FRAME); + } + + if( has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ) + { + STORE_JACOBIAN_N( i_var_calobject_warp, + dq_dcalobject_warp[i_pt * 2 + i_xy].values, + weight * SCALE_CALOBJECT_WARP, + MRCAL_NSTATE_CALOBJECT_WARP); + } + + iMeasurement++; + } + } + else + { + // Outlier. + + // This is arbitrary. I'm skipping this observation, so I don't + // touch the projection results, and I set the measurement and + // all its gradients to 0. I need to have SOME dependency on the + // frame parameters to ensure a full-rank Hessian, so if we're + // skipping all observations for this frame the system will + // become singular. I don't currently handle this. libdogleg + // will complain loudly, and add small diagonal L2 + // regularization terms + for( int i_xy=0; i_xy<2; i_xy++ ) + { + const double err = 0.0; + + if(ctx->verbose) + MSG( "obs/frame/cam_i/cam_e/dot: %d %d %d %d %d err: %g", + i_observation_board, iframe, icam_intrinsics, icam_extrinsics, i_pt, err); + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = err; + norm2_error += err*err; + + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + STORE_JACOBIAN( i_var_intrinsics + i_xy, 0.0 ); + STORE_JACOBIAN( i_var_intrinsics + i_xy+2, 0.0 ); + } + + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + if(gradient_sparse_meta.pool != NULL) + { + const int ivar0 = dq_dintrinsics_pool_int[splined_intrinsics_grad_irun] - + ( ctx->problem_selections.do_optimize_intrinsics_core ? 0 : 4 ); + const int len = gradient_sparse_meta.run_side_length; + const int ivar_stridey = gradient_sparse_meta.ivar_stridey; + + for(int iy=0; iyNintrinsics-Ncore; i++) + STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, 0.0 ); + } + } + + if( ctx->problem_selections.do_optimize_extrinsics ) + if( icam_extrinsics >= 0 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, 0.0, 0.0, 0.0); + STORE_JACOBIAN3( i_var_camera_rt + 3, 0.0, 0.0, 0.0); + } + + if( ctx->problem_selections.do_optimize_frames ) + { + // Arbitrary differences between the dimensions to keep + // my Hessian non-singular. This is 100% arbitrary. I'm + // skipping these measurements so these variables + // actually don't affect the computation at all + STORE_JACOBIAN3( i_var_frame_rt + 0, 0,0,0); + STORE_JACOBIAN3( i_var_frame_rt + 3, 0,0,0); + } + + if( has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ) + STORE_JACOBIAN_N( i_var_calobject_warp, + (double*)NULL, 0.0, + MRCAL_NSTATE_CALOBJECT_WARP); + + iMeasurement++; + } + } + if(gradient_sparse_meta.pool != NULL) + splined_intrinsics_grad_irun++; + } + } + + // Handle all the point observations. This is VERY similar to the + // board-observation loop above. Please consolidate + for(int i_observation_point = 0; + i_observation_point < ctx->Nobservations_point; + i_observation_point++) + { + const mrcal_observation_point_t* observation = &ctx->observations_point[i_observation_point]; + + const int icam_intrinsics = observation->icam.intrinsics; + const int icam_extrinsics = observation->icam.extrinsics; + const int i_point = observation->i_point; + const bool use_position_from_state = + ctx->problem_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]; + double weight = qx_qy_w__observed->z; + + if(weight <= 0.0) + { + // Outlier. Cost = 0. Jacobians are 0 too, but I must preserve the + // structure + const int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + // invalid if icam_extrinsics < 0, but unused in that case + const int i_var_camera_rt = + mrcal_state_index_extrinsics(icam_extrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + const int i_var_point = + mrcal_state_index_points(i_point, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + + // I have my two measurements (dx, dy). I propagate their + // gradient and store them + for( int i_xy=0; i_xy<2; i_xy++ ) + { + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = 0; + + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + // fx,fy. x depends on fx only. y depends on fy only + STORE_JACOBIAN( i_var_intrinsics + i_xy, 0 ); + + // cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only + STORE_JACOBIAN( i_var_intrinsics + i_xy+2, 0); + } + + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + if( (ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions) && + ctx->lensmodel.type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC ) + { + // sparse gradient. This is an outlier, so it doesn't + // matter which points I say I depend on, as long as I + // pick the right number, and says that j=0. I pick the + // control points at the start because why not + const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config = + &ctx->lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config; + int runlen = config->order+1; + for(int i=0; iNintrinsics-Ncore; i++) + STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, 0); + } + + if(icam_extrinsics >= 0 && ctx->problem_selections.do_optimize_extrinsics ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, 0,0,0 ); + STORE_JACOBIAN3( i_var_camera_rt + 3, 0,0,0 ); + } + + if( use_position_from_state ) + STORE_JACOBIAN3( i_var_point, 0,0,0 ); + + iMeasurement++; + } + + // 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 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, 0,0,0 ); + STORE_JACOBIAN3( i_var_camera_rt + 3, 0,0,0 ); + } + if( use_position_from_state ) + STORE_JACOBIAN3( i_var_point, 0,0,0 ); + iMeasurement++; +#endif + + continue; + } + + + const int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + // invalid if icam_extrinsics < 0, but unused in that case + const int i_var_camera_rt = + mrcal_state_index_extrinsics(icam_extrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + const int i_var_point = + mrcal_state_index_points(i_point, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_selections, &ctx->lensmodel); + mrcal_point3_t point_ref; + if(use_position_from_state) + unpack_solver_state_point_one(&point_ref, &packed_state[i_var_point]); + else + point_ref = ctx->points[i_point]; + + int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics); + + // WARNING: "compute size(dq_dintrinsics_pool_double) correctly and maybe bounds-check" + std::vector dq_dintrinsics_pool_double(Ngradients); + // used for LENSMODEL_SPLINED_STEREOGRAPHIC only, but getting rid of + // this in other cases isn't worth the trouble + int dq_dintrinsics_pool_int [1]; + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; + + mrcal_point3_t dq_drcamera[2]; + mrcal_point3_t dq_dtcamera[2]; + mrcal_point3_t dq_dpoint [2]; + + // The array reference [-3] is intended, but the compiler throws a + // warning. I silence it here +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Warray-bounds" + mrcal_point2_t q_hypothesis; + project(&q_hypothesis, + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_double.data() : NULL, + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_int : NULL, + &dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta, + + ctx->problem_selections.do_optimize_extrinsics ? + dq_drcamera : NULL, + ctx->problem_selections.do_optimize_extrinsics ? + dq_dtcamera : NULL, + NULL, // frame rotation. I only have a point position + use_position_from_state ? dq_dpoint : NULL, + NULL, + + // input + intrinsics_all[icam_intrinsics].data(), + &camera_rt[icam_extrinsics], + + // I only have the point position, so the 'rt' memory + // points 3 back. The fake "r" here will not be + // referenced + (mrcal_pose_t*)(&point_ref.xyz[-3]), + NULL, + + icam_extrinsics < 0, + &ctx->lensmodel, &ctx->precomputed, + 0,0,0); +#pragma GCC diagnostic pop + + // I have my two measurements (dx, dy). I propagate their + // gradient and store them + for( int i_xy=0; i_xy<2; i_xy++ ) + { + const double err = (q_hypothesis.xy[i_xy] - qx_qy_w__observed->xyz[i_xy])*weight; + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = err; + norm2_error += err*err; + + if( ctx->problem_selections.do_optimize_intrinsics_core ) + { + // fx,fy. x depends on fx only. y depends on fy only + STORE_JACOBIAN( i_var_intrinsics + i_xy, + dq_dfxy[i_xy] * + weight * SCALE_INTRINSICS_FOCAL_LENGTH ); + + // cx,cy. The gradients here are known to be 1. And x depends on cx only. And y depends on cy only + STORE_JACOBIAN( i_var_intrinsics + i_xy+2, + weight * SCALE_INTRINSICS_CENTER_PIXEL ); + } + + if( ctx->problem_selections.do_optimize_intrinsics_distortions ) + { + if(gradient_sparse_meta.pool != NULL) + { + // u = stereographic(p) + // q = (u + deltau(u)) * f + c + // + // Intrinsics: + // dq/diii = f ddeltau/diii + // + // ddeltau/diii = flatten(ABCDx[0..3] * ABCDy[0..3]) + const int ivar0 = dq_dintrinsics_pool_int[0] - + ( ctx->problem_selections.do_optimize_intrinsics_core ? 0 : 4 ); + + const int len = gradient_sparse_meta.run_side_length; + const double* ABCDx = &gradient_sparse_meta.pool[0]; + const double* ABCDy = &gradient_sparse_meta.pool[len]; + + const int ivar_stridey = gradient_sparse_meta.ivar_stridey; + const double* fxy = &intrinsics_all[icam_intrinsics][0]; + + for(int iy=0; iyNintrinsics-Ncore; i++) + STORE_JACOBIAN( i_var_intrinsics+Ncore_state + i, + dq_dintrinsics_nocore[i_xy*(ctx->Nintrinsics-Ncore) + + i] * + weight * SCALE_DISTORTION ); + } + } + + if( ctx->problem_selections.do_optimize_extrinsics ) + if( icam_extrinsics >= 0 ) + { + STORE_JACOBIAN3( i_var_camera_rt + 0, + dq_drcamera[i_xy].xyz[0] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_xy].xyz[1] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_xy].xyz[2] * + weight * SCALE_ROTATION_CAMERA); + STORE_JACOBIAN3( i_var_camera_rt + 3, + dq_dtcamera[i_xy].xyz[0] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_xy].xyz[1] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_xy].xyz[2] * + weight * SCALE_TRANSLATION_CAMERA); + } + + if( use_position_from_state ) + STORE_JACOBIAN3( i_var_point, + dq_dpoint[i_xy].xyz[0] * + weight * SCALE_POSITION_POINT, + dq_dpoint[i_xy].xyz[1] * + weight * SCALE_POSITION_POINT, + dq_dpoint[i_xy].xyz[2] * + weight * SCALE_POSITION_POINT); + + iMeasurement++; + } + + + // 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) + { + double distsq = + point_ref.x*point_ref.x + + point_ref.y*point_ref.y + + point_ref.z*point_ref.z; + double penalty, dpenalty_ddistsq; + if(model_supports_projection_behind_camera(&ctx->lensmodel) || + point_ref.z > 0.0) + penalty_range_normalization(&penalty, &dpenalty_ddistsq, distsq, ctx,weight); + else + { + penalty_range_normalization(&penalty, &dpenalty_ddistsq, -distsq, ctx,weight); + dpenalty_ddistsq *= -1.; + } + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = penalty; + norm2_error += penalty*penalty; + + if( use_position_from_state ) + { + double scale = 2.0 * dpenalty_ddistsq * SCALE_POSITION_POINT; + STORE_JACOBIAN3( i_var_point, + scale*point_ref.x, + scale*point_ref.y, + scale*point_ref.z ); + } + + iMeasurement++; + } + else + { + // I need to transform the point. I already computed + // this stuff in project()... + double Rc[3*3]; + double d_Rc_rc[9*3]; + + mrcal_R_from_r(Rc, + d_Rc_rc, + camera_rt[icam_extrinsics].r.xyz); + + mrcal_point3_t pcam; + mul_vec3_gen33t_vout(point_ref.xyz, Rc, pcam.xyz); + add_vec(3, pcam.xyz, camera_rt[icam_extrinsics].t.xyz); + + double distsq = + pcam.x*pcam.x + + pcam.y*pcam.y + + pcam.z*pcam.z; + double penalty, dpenalty_ddistsq; + if(model_supports_projection_behind_camera(&ctx->lensmodel) || + pcam.z > 0.0) + penalty_range_normalization(&penalty, &dpenalty_ddistsq, distsq, ctx,weight); + else + { + penalty_range_normalization(&penalty, &dpenalty_ddistsq, -distsq, ctx,weight); + dpenalty_ddistsq *= -1.; + } + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + x[iMeasurement] = penalty; + norm2_error += penalty*penalty; + + if( ctx->problem_selections.do_optimize_extrinsics ) + { + // pcam.x = Rc[row0]*point*SCALE + tc + // d(pcam.x)/dr = d(Rc[row0])/drc*point*SCALE + // d(Rc[row0])/drc is 3x3 matrix at &d_Rc_rc[0] + double d_ptcamx_dr[3]; + double d_ptcamy_dr[3]; + double d_ptcamz_dr[3]; + mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*0], d_ptcamx_dr ); + mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*1], d_ptcamy_dr ); + mul_vec3_gen33_vout( point_ref.xyz, &d_Rc_rc[9*2], d_ptcamz_dr ); + + STORE_JACOBIAN3( i_var_camera_rt + 0, + SCALE_ROTATION_CAMERA* + 2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[0] + + pcam.y*d_ptcamy_dr[0] + + pcam.z*d_ptcamz_dr[0] ), + SCALE_ROTATION_CAMERA* + 2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[1] + + pcam.y*d_ptcamy_dr[1] + + pcam.z*d_ptcamz_dr[1] ), + SCALE_ROTATION_CAMERA* + 2.0*dpenalty_ddistsq*( pcam.x*d_ptcamx_dr[2] + + pcam.y*d_ptcamy_dr[2] + + pcam.z*d_ptcamz_dr[2] ) ); + STORE_JACOBIAN3( i_var_camera_rt + 3, + SCALE_TRANSLATION_CAMERA* + 2.0*dpenalty_ddistsq*pcam.x, + SCALE_TRANSLATION_CAMERA* + 2.0*dpenalty_ddistsq*pcam.y, + SCALE_TRANSLATION_CAMERA* + 2.0*dpenalty_ddistsq*pcam.z ); + } + + if( use_position_from_state ) + STORE_JACOBIAN3( i_var_point, + SCALE_POSITION_POINT* + 2.0*dpenalty_ddistsq*(pcam.x*Rc[0] + pcam.y*Rc[3] + pcam.z*Rc[6]), + SCALE_POSITION_POINT* + 2.0*dpenalty_ddistsq*(pcam.x*Rc[1] + pcam.y*Rc[4] + pcam.z*Rc[7]), + SCALE_POSITION_POINT* + 2.0*dpenalty_ddistsq*(pcam.x*Rc[2] + pcam.y*Rc[5] + pcam.z*Rc[8]) ); + iMeasurement++; + } +#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 ) + { + 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); + } + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #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" +#endif + + 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; + + 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; + } + + int Nmeasurements_nonregularization = + ctx->Nmeasurements - + (Nmeasurements_regularization_distortion + + Nmeasurements_regularization_centerpixel + + Nmeasurements_regularization_unity_cam01); + + double normal_pixel_error = 1.0; + double expected_total_pixel_error_sq = + (double)Nmeasurements_nonregularization * + normal_pixel_error * + normal_pixel_error; + if(dump_regularizaton_details) + MSG("expected_total_pixel_error_sq: %f", expected_total_pixel_error_sq); + + // 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; + + if(ctx->problem_selections.do_apply_regularization && + (ctx->problem_selections.do_optimize_intrinsics_distortions || + ctx->problem_selections.do_optimize_intrinsics_core)) + { + double scale_regularization_distortion = 0.0; + double scale_regularization_centerpixel = 0.0; + + // compute scales + { + 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; + + if(dump_regularizaton_details) + MSG("expected_regularization_distortion_error_sq: %f", expected_regularization_distortion_error_sq_noscale*scale_sq); + + scale_regularization_distortion = sqrt(scale_sq); + } + + if(modelHasCore_fxfycxcy(&ctx->lensmodel) && + ctx->problem_selections.do_optimize_intrinsics_core) + { + double normal_centerpixel_offset = 500.0; + + double expected_regularization_centerpixel_error_sq_noscale = + (double)Nmeasurements_regularization_centerpixel * + normal_centerpixel_offset * + normal_centerpixel_offset; + + double scale_sq = + expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_centerpixel_error_sq_noscale; + + if(dump_regularizaton_details) + MSG("expected_regularization_centerpixel_error_sq: %f", expected_regularization_centerpixel_error_sq_noscale*scale_sq); + + 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++) + { + 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++) + { + // 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.; + } + + if(Jt) Jrowptr[iMeasurement] = iJacobian; + double err = scale*intrinsics_all[icam_intrinsics][j+Ncore]; + x[iMeasurement] = err; + norm2_error += err*err; + + STORE_JACOBIAN( i_var_intrinsics + Ncore_state + j, + scale * SCALE_DISTORTION ); + + iMeasurement++; + if(dump_regularizaton_details) + MSG("regularization distortion: %g; norm2: %g", err, err*err); + + } + } + } + + if( 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); + + // 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; + + // compute scales + { +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: better unity_cam01 scale" +#endif + double normal_unity_cam01_value = 1.0; + + double expected_regularization_unity_cam01_error_sq_noscale = + (double)Nmeasurements_regularization_unity_cam01 * + normal_unity_cam01_value * + normal_unity_cam01_value; + + double scale_sq = + expected_total_pixel_error_sq * 0.005/(double)Nregularization_types / expected_regularization_unity_cam01_error_sq_noscale; + + 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); + } + + // 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); + + 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; + + 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]); + + iMeasurement++; + if(dump_regularizaton_details) + MSG("regularization unity_cam01: %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 ) + { + MSG("Assertion (iJacobian == ctx->N_j_nonzero ) failed: (%d != %d)", + iJacobian, ctx->N_j_nonzero); + assert(0); + } +} + +bool mrcal_optimizer_callback(// out + + // These output pointers may NOT be NULL, unlike + // their analogues in mrcal_optimize() + + // Shape (Nstate,) + 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, + + // Shape (Nmeasurements,) + double* x, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_x, + + // output Jacobian. May be NULL if we don't need + // it. This is the unitless Jacobian, used by the + // internal optimization routines + cholmod_sparse* Jt, + + + // in + + // intrinsics is a concatenation of the intrinsics core + // and the distortion params. The specific distortion + // parameters may vary, depending on lensmodel, so + // this is a variable-length structure + const double* intrinsics, // Ncameras_intrinsics * NlensParams + const mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame + const mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame + const mrcal_point3_t* points, // Npoints of these. In the reference frame + const mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + 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 + // expected to be 1.0. Less precise observations + // have lower weights. + // + // 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 + + mrcal_problem_selections_t problem_selections, + const mrcal_problem_constants_t* problem_constants, + + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n, + bool verbose) +{ + 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."); + return result; + } + + if( Nobservations_board > 0 ) + { + if( problem_selections.do_optimize_calobject_warp && calobject_warp == NULL ) + { + MSG("ERROR: We're optimizing the calibration object warp, so a buffer with a seed MUST be passed in."); + return result; + } + } + else + problem_selections.do_optimize_calobject_warp = false; + + if(!modelHasCore_fxfycxcy(lensmodel)) + problem_selections.do_optimize_intrinsics_core = false; + + if(!problem_selections.do_optimize_intrinsics_core && + !problem_selections.do_optimize_intrinsics_distortions && + !problem_selections.do_optimize_extrinsics && + !problem_selections.do_optimize_frames && + !problem_selections.do_optimize_calobject_warp) + { + MSG("Not optimizing any of our variables!"); + return result; + } + + + const int Nstate = mrcal_num_states(Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, + problem_selections, + lensmodel); + if( buffer_size_b_packed != Nstate*(int)sizeof(double) ) + { + MSG("The buffer passed to fill-in b_packed has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nstate*(int)sizeof(double),buffer_size_b_packed); + return result; + } + + 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, + Nframes, + Npoints, Npoints_fixed, + problem_selections, + lensmodel); + 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, + Nframes, + Npoints, Npoints_fixed, + observations_board, + observations_point, + problem_selections, + lensmodel); + + if( buffer_size_x != Nmeasurements*(int)sizeof(double) ) + { + MSG("The buffer passed to fill-in x has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nmeasurements*(int)sizeof(double),buffer_size_x); + return result; + } + + const int Npoints_fromBoards = + Nobservations_board * + calibration_object_width_n*calibration_object_height_n; + + const callback_context_t ctx = { + .intrinsics = intrinsics, + .extrinsics_fromref = extrinsics_fromref, + .frames_toref = frames_toref, + .points = points, + .calobject_warp = calobject_warp, + .Ncameras_intrinsics = Ncameras_intrinsics, + .Ncameras_extrinsics = Ncameras_extrinsics, + .Nframes = Nframes, + .Npoints = Npoints, + .Npoints_fixed = Npoints_fixed, + .observations_board = observations_board, + .observations_board_pool = observations_board_pool, + .Nobservations_board = Nobservations_board, + .observations_point = observations_point, + .observations_point_pool = observations_point_pool, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, + .verbose = verbose, + .lensmodel = *lensmodel, + .imagersizes = imagersizes, + .problem_selections = problem_selections, + .problem_constants = problem_constants, + .calibration_object_spacing = calibration_object_spacing, + .calibration_object_width_n = calibration_object_width_n > 0 ? calibration_object_width_n : 0, + .calibration_object_height_n= calibration_object_height_n > 0 ? calibration_object_height_n : 0, + .Nmeasurements = Nmeasurements, + .N_j_nonzero = N_j_nonzero, + .Nintrinsics = Nintrinsics}; + _mrcal_precompute_lensmodel_data((mrcal_projection_precomputed_t*)&ctx.precomputed, lensmodel); + + pack_solver_state(b_packed, + lensmodel, intrinsics, + extrinsics_fromref, + frames_toref, + points, + calobject_warp, + problem_selections, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, Npoints-Npoints_fixed, + Nobservations_board, + Nstate); + + optimizer_callback(b_packed, x, Jt, &ctx); + + result = true; + + return result; +} + +mrcal_stats_t +mrcal_optimize( // out + // Each one of these output pointers may be NULL + + // Shape (Nstate,) + double* b_packed_final, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_b_packed_final, + + // Shape (Nmeasurements,) + double* x_final, + // used only to confirm that the user passed-in the buffer they + // should have passed-in. The size must match exactly + int buffer_size_x_final, + + // out, in + + // These are a seed on input, solution on output + + // intrinsics is a concatenation of the intrinsics core and the + // distortion params. The specific distortion parameters may + // vary, depending on lensmodel, so this is a variable-length + // structure + double* intrinsics, // Ncameras_intrinsics * NlensParams + mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame + mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame + mrcal_point3_t* points, // Npoints of these. In the reference frame + mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp + + // in + int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes, + int Npoints, int Npoints_fixed, // at the end of points[] + + const mrcal_observation_board_t* observations_board, + const mrcal_observation_point_t* observations_point, + int Nobservations_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 + // expected to be 1.0, which implies that the noise on the + // observation has standard deviation of + // observed_pixel_uncertainty. observed_pixel_uncertainty scales + // inversely with the weight. + // + // z<0 indicates that this is an outlier. This is respected on + // input (even if !do_apply_outlier_rejection). New outliers are + // marked with z<0 on output, so this isn't const + mrcal_point3_t* observations_board_pool, + + // 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, + const mrcal_problem_constants_t* problem_constants, + + double calibration_object_spacing, + int calibration_object_width_n, + int calibration_object_height_n, + bool verbose, + + bool check_gradient) +{ + if( Nobservations_board > 0 ) + { + if( problem_selections.do_optimize_calobject_warp && calobject_warp == NULL ) + { + MSG("ERROR: We're optimizing the calibration object warp, so a buffer with a seed MUST be passed in."); + return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0}; + } + } + else + problem_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}; +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: can I loosen this? Optimizing intrinsics and triangulated points together should work" +#endif + } + + if(!modelHasCore_fxfycxcy(lensmodel)) + problem_selections.do_optimize_intrinsics_core = false; + + if(!problem_selections.do_optimize_intrinsics_core && + !problem_selections.do_optimize_intrinsics_distortions && + !problem_selections.do_optimize_extrinsics && + !problem_selections.do_optimize_frames && + !problem_selections.do_optimize_calobject_warp) + { + MSG("Warning: Not optimizing any of our variables"); + } + + dogleg_parameters2_t dogleg_parameters; + dogleg_getDefaultParameters(&dogleg_parameters); + dogleg_parameters.dogleg_debug = verbose ? DOGLEG_DEBUG_VNLOG : 0; + + // These were derived empirically, seeking high accuracy, fast convergence + // and without serious concern for performance. I looked only at a single + // frame. Tweak them please + dogleg_parameters.Jt_x_threshold = 0; + dogleg_parameters.update_threshold = 1e-6; + dogleg_parameters.trustregion_threshold = 0; + dogleg_parameters.max_iterations = 300; + // dogleg_parameters.trustregion_decrease_factor = 0.1; + // dogleg_parameters.trustregion_decrease_threshold = 0.15; + // dogleg_parameters.trustregion_increase_factor = 4.0 + // dogleg_parameters.trustregion_increase_threshold = 0.75; + + const int Npoints_fromBoards = + Nobservations_board * + calibration_object_width_n*calibration_object_height_n; + + callback_context_t ctx = { + .intrinsics = intrinsics, + .extrinsics_fromref = extrinsics_fromref, + .frames_toref = frames_toref, + .points = points, + .calobject_warp = calobject_warp, + .Ncameras_intrinsics = Ncameras_intrinsics, + .Ncameras_extrinsics = Ncameras_extrinsics, + .Nframes = Nframes, + .Npoints = Npoints, + .Npoints_fixed = Npoints_fixed, + .observations_board = observations_board, + .observations_board_pool = observations_board_pool, + .Nobservations_board = Nobservations_board, + .observations_point = observations_point, + .observations_point_pool = observations_point_pool, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, + .verbose = verbose, + .lensmodel = *lensmodel, + .imagersizes = imagersizes, + .problem_selections = problem_selections, + .problem_constants = problem_constants, + .calibration_object_spacing = calibration_object_spacing, + .calibration_object_width_n = calibration_object_width_n > 0 ? calibration_object_width_n : 0, + .calibration_object_height_n= calibration_object_height_n > 0 ? calibration_object_height_n : 0, + .Nmeasurements = mrcal_num_measurements(Nobservations_board, + Nobservations_point, + observations_point_triangulated, + Nobservations_point_triangulated, + calibration_object_width_n, + calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, + problem_selections, + 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, + Nframes, + Npoints, Npoints_fixed, + observations_board, + observations_point, + problem_selections, + lensmodel), + .Nintrinsics = mrcal_lensmodel_num_params(lensmodel)}; + _mrcal_precompute_lensmodel_data((mrcal_projection_precomputed_t*)&ctx.precomputed, lensmodel); + + const int Nstate = mrcal_num_states(Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, + problem_selections, + lensmodel); + + if( b_packed_final != NULL && + buffer_size_b_packed_final != Nstate*(int)sizeof(double) ) + { + MSG("The buffer passed to fill-in b_packed_final has the wrong size. Needed exactly %d bytes, but got %d bytes", + Nstate*(int)sizeof(double),buffer_size_b_packed_final); + return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0}; + } + if( x_final != NULL && + buffer_size_x_final != ctx.Nmeasurements*(int)sizeof(double) ) + { + MSG("The buffer passed to fill-in x_final has the wrong size. Needed exactly %d bytes, but got %d bytes", + ctx.Nmeasurements*(int)sizeof(double),buffer_size_x_final); + return (mrcal_stats_t){.rms_reproj_error__pixels = -1.0}; + } + + + dogleg_solverContext_t* solver_context = NULL; + + if(verbose) + MSG("## Nmeasurements=%d, Nstate=%d", ctx.Nmeasurements, Nstate); + if(ctx.Nmeasurements <= Nstate) + { + MSG("WARNING: problem isn't overdetermined: Nmeasurements=%d, Nstate=%d. Solver may not converge, and if it does, the results aren't reliable. Add more constraints and/or regularization", + ctx.Nmeasurements, Nstate); + } + + // WARNING: is it reasonable to put this on the stack? Can I use + // b_packed_final for this? + std::vector packed_state(Nstate); + pack_solver_state(packed_state.data(), + lensmodel, intrinsics, + extrinsics_fromref, + frames_toref, + points, + calobject_warp, + problem_selections, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, Npoints-Npoints_fixed, + Nobservations_board, + Nstate); + + double norm2_error = -1.0; + mrcal_stats_t stats = {.rms_reproj_error__pixels = -1.0 }; + + 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, + observations_board, + Nobservations_board, + Nobservations_point, +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: not const for now. this will become const once the outlier bit moves to the point_triangulated pool" +#endif + (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, + Nmeasurements_board, + (double)(stats.Noutliers_board * 100) / (double)Nmeasurements_board); + return true;}())); +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: the above print should deal with triangulated points too" +#endif + + + // Done. I have the final state. I spit it back out + unpack_solver_state( intrinsics, // Ncameras_intrinsics of these + extrinsics_fromref, // Ncameras_extrinsics of these + frames_toref, // Nframes of these + points, // Npoints of these + calobject_warp, + packed_state.data(), + lensmodel, + problem_selections, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, Npoints-Npoints_fixed, + Nobservations_board, + Nstate); + + 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, + calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, + Nframes, + Npoints, Npoints_fixed, Nobservations_board, Nobservations_point, + problem_selections, + lensmodel); + + if(imeas_reg0 >= 0 && verbose) + { + // we have regularization + const double* xreg = &solver_context->beforeStep->x[imeas_reg0]; + + int Ncore = modelHasCore_fxfycxcy(lensmodel) ? 4 : 0; + + int Nmeasurements_regularization_distortion = 0; + if(problem_selections.do_optimize_intrinsics_distortions) + Nmeasurements_regularization_distortion = + Ncameras_intrinsics*(ctx.Nintrinsics-Ncore); + + int Nmeasurements_regularization_centerpixel = 0; + if(problem_selections.do_optimize_intrinsics_core) + Nmeasurements_regularization_centerpixel = + Ncameras_intrinsics*2; + + double norm2_err_regularization_distortion = 0; + double norm2_err_regularization_centerpixel = 0; + + for(int i=0; i 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); + } + + 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) + { + MSG("reg err ratio (unity_cam01): %.3g", + regularization_ratio_unity_cam01); + } + } + } + else + for(int ivar=0; ivarbeforeStep->p, Nstate*sizeof(double)); + if(x_final) + memcpy(x_final, solver_context->beforeStep->x, ctx.Nmeasurements*sizeof(double)); + + if(solver_context != NULL) + dogleg_freeContext(&solver_context); + + return stats; +} + +bool mrcal_write_cameramodel_file(const char* filename, + const mrcal_cameramodel_t* cameramodel) +{ + bool result = false; + FILE* fp = fopen(filename, "w"); + if(fp == NULL) + { + MSG("Couldn't open('%s')", filename); + return false; + } + + char lensmodel_string[1024]; + if(!mrcal_lensmodel_name(lensmodel_string, sizeof(lensmodel_string), + &cameramodel->lensmodel)) + { + MSG("Couldn't construct lensmodel string. Unconfigured string: '%s'", + mrcal_lensmodel_name_unconfigured(&cameramodel->lensmodel)); + if(fp != NULL) + fclose(fp); + return result; + } + + int Nparams = mrcal_lensmodel_num_params(&cameramodel->lensmodel); + if(Nparams<0) + { + MSG("Couldn't get valid Nparams from lensmodel string '%s'", + lensmodel_string); + if(fp != NULL) + fclose(fp); + return result;; + } + + fprintf(fp, "{\n"); + fprintf(fp, " 'lensmodel': '%s',\n", lensmodel_string); + fprintf(fp, " 'intrinsics': [ "); + for(int i=0; iintrinsics[i]); + fprintf(fp, "],\n"); + fprintf(fp, " 'imagersize': [ %u, %u ],\n", + cameramodel->imagersize[0], + cameramodel->imagersize[1]); + fprintf(fp, " 'extrinsics': [ %f, %f, %f, %f, %f, %f ]\n", + cameramodel->rt_cam_ref[0], + cameramodel->rt_cam_ref[1], + cameramodel->rt_cam_ref[2], + cameramodel->rt_cam_ref[3], + cameramodel->rt_cam_ref[4], + cameramodel->rt_cam_ref[5]); + + fprintf(fp,"}\n"); + result = true; + + if(fp != NULL) + fclose(fp); + return result; +} + + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: fixed points should live in a separate array, instead of at the end of the 'points' array" +#endif diff --git a/wpical/src/main/native/thirdparty/mrcal/src/opencv.c b/wpical/src/main/native/thirdparty/mrcal/src/opencv.c new file mode 100644 index 00000000000..3e751f611b1 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/opencv.c @@ -0,0 +1,152 @@ +// 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-opencv.c b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c new file mode 100644 index 00000000000..a0df80b52a7 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c @@ -0,0 +1,155 @@ +// The implementation of mrcal_R_from_r is based on opencv. +// The sources have been heavily modified, but the opencv logic remains. +// +// from opencv-4.1.2+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 + +// Apparently I need this in MSVC to get constants +#define _USE_MATH_DEFINES + +#include +#include + +#include "poseutils.h" +#include "strides.h" + +void mrcal_R_from_r_full( // outputs + double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1, // 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,) vector + int r_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(R, 3,3); + init_stride_3D(J, 3,3,3 ); + init_stride_1D(r, 3 ); + + double norm2r = 0.0; + for(int i=0; i<3; i++) + norm2r += P1(r,i)*P1(r,i); + + if( norm2r < DBL_EPSILON*DBL_EPSILON ) + { + mrcal_identity_R_full(R, R_stride0, R_stride1); + + if( J ) + { + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + for(int k=0; k<3; k++) + P3(J,i,j,k) = 0.; + + P3(J,1,2,0) = -1.; + P3(J,2,0,1) = -1.; + P3(J,0,1,2) = -1.; + + P3(J,2,1,0) = 1.; + P3(J,0,2,1) = 1.; + P3(J,1,0,2) = 1.; + } + return; + } + + double theta = sqrt(norm2r); + + double s = sin(theta); + double c = cos(theta); + double c1 = 1. - c; + double itheta = 1./theta; + + double r_unit[3]; + for(int i=0; i<3; i++) + r_unit[i] = P1(r,i) * itheta; + + // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x] + P2(R, 0,0) = c + c1*r_unit[0]*r_unit[0]; + P2(R, 0,1) = c1*r_unit[0]*r_unit[1] - s*r_unit[2]; + P2(R, 0,2) = c1*r_unit[0]*r_unit[2] + s*r_unit[1]; + P2(R, 1,0) = c1*r_unit[0]*r_unit[1] + s*r_unit[2]; + P2(R, 1,1) = c + c1*r_unit[1]*r_unit[1]; + P2(R, 1,2) = c1*r_unit[1]*r_unit[2] - s*r_unit[0]; + P2(R, 2,0) = c1*r_unit[0]*r_unit[2] - s*r_unit[1]; + P2(R, 2,1) = c1*r_unit[1]*r_unit[2] + s*r_unit[0]; + P2(R, 2,2) = c + c1*r_unit[2]*r_unit[2]; + + if( J ) + { + // opencv had some logic with lots of 0s. I unrolled all of the + // loops, and removed all the resulting 0 terms + double a0, a1, a3; + double a2 = itheta * c1; + double a4 = itheta * s; + + a0 = -s *r_unit[0]; + a1 = (s - 2*a2)*r_unit[0]; + a3 = (c - a4)*r_unit[0]; + P3(J,0,0,0) = a0 + a1*r_unit[0]*r_unit[0] + a2*(r_unit[0]+r_unit[0]); + P3(J,0,1,0) = a1*r_unit[0]*r_unit[1] + a2*r_unit[1] - a3*r_unit[2]; + P3(J,0,2,0) = a1*r_unit[0]*r_unit[2] + a2*r_unit[2] + a3*r_unit[1]; + P3(J,1,0,0) = a1*r_unit[0]*r_unit[1] + a2*r_unit[1] + a3*r_unit[2]; + P3(J,1,1,0) = a0 + a1*r_unit[1]*r_unit[1]; + P3(J,1,2,0) = a1*r_unit[1]*r_unit[2] - a3*r_unit[0] - a4; + P3(J,2,0,0) = a1*r_unit[0]*r_unit[2] + a2*r_unit[2] - a3*r_unit[1]; + P3(J,2,1,0) = a1*r_unit[1]*r_unit[2] + a3*r_unit[0] + a4; + P3(J,2,2,0) = a0 + a1*r_unit[2]*r_unit[2]; + + a0 = -s *r_unit[1]; + a1 = (s - 2*a2)*r_unit[1]; + a3 = (c - a4)*r_unit[1]; + P3(J,0,0,1) = a0 + a1*r_unit[0]*r_unit[0]; + P3(J,0,1,1) = a1*r_unit[0]*r_unit[1] + a2*r_unit[0] - a3*r_unit[2]; + P3(J,0,2,1) = a1*r_unit[0]*r_unit[2] + a3*r_unit[1] + a4; + P3(J,1,0,1) = a1*r_unit[0]*r_unit[1] + a2*r_unit[0] + a3*r_unit[2]; + P3(J,1,1,1) = a0 + a1*r_unit[1]*r_unit[1] + a2*(r_unit[1]+r_unit[1]); + P3(J,1,2,1) = a1*r_unit[1]*r_unit[2] + a2*r_unit[2] - a3*r_unit[0]; + P3(J,2,0,1) = a1*r_unit[0]*r_unit[2] - a3*r_unit[1] - a4; + P3(J,2,1,1) = a1*r_unit[1]*r_unit[2] + a2*r_unit[2] + a3*r_unit[0]; + P3(J,2,2,1) = a0 + a1*r_unit[2]*r_unit[2]; + + a0 = -s *r_unit[2]; + a1 = (s - 2*a2)*r_unit[2]; + a3 = (c - a4)*r_unit[2]; + P3(J,0,0,2) = a0 + a1*r_unit[0]*r_unit[0]; + P3(J,0,1,2) = a1*r_unit[0]*r_unit[1] - a3*r_unit[2] - a4; + P3(J,0,2,2) = a1*r_unit[0]*r_unit[2] + a2*r_unit[0] + a3*r_unit[1]; + P3(J,1,0,2) = a1*r_unit[0]*r_unit[1] + a3*r_unit[2] + a4; + P3(J,1,1,2) = a0 + a1*r_unit[1]*r_unit[1]; + P3(J,1,2,2) = a1*r_unit[1]*r_unit[2] + a2*r_unit[1] - a3*r_unit[0]; + P3(J,2,0,2) = a1*r_unit[0]*r_unit[2] + a2*r_unit[0] - a3*r_unit[1]; + P3(J,2,1,2) = a1*r_unit[1]*r_unit[2] + a2*r_unit[1] + a3*r_unit[0]; + P3(J,2,2,2) = a0 + a1*r_unit[2]*r_unit[2] + a2*(r_unit[2]+r_unit[2]); + } +} 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 new file mode 100644 index 00000000000..deb4670887e --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp @@ -0,0 +1,891 @@ +// 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 "autodiff.hh" +#include "strides.h" + +extern "C" { +#include "poseutils.h" +} + +template +static void +rotate_point_r_core(// output + val_withgrad_t* x_outg, + + // inputs + const val_withgrad_t* rg, + const val_withgrad_t* x_ing, + bool inverted) +{ + // 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) -> + // 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 + double sign = inverted ? -1.0 : 1.0; + + const val_withgrad_t th2 = + rg[0]*rg[0] + + rg[1]*rg[1] + + rg[2]*rg[2]; + const val_withgrad_t cross[3] = + { + (rg[1]*x_ing[2] - rg[2]*x_ing[1])*sign, + (rg[2]*x_ing[0] - rg[0]*x_ing[2])*sign, + (rg[0]*x_ing[1] - rg[1]*x_ing[0])*sign + }; + const val_withgrad_t inner = + rg[0]*x_ing[0] + + rg[1]*x_ing[1] + + rg[2]*x_ing[2]; + + if(th2.x < 1e-10) + { + // Small rotation. I don't want to divide by 0, so I take the limit + // lim(th->0, xrot) = + // = x + cross(r, x) + r rt x lim(th->0, (1 - cos(th)) / (th*th)) + // = x + cross(r, x) + r rt x lim(th->0, sin(th) / (2*th)) + // = x + cross(r, x) + r rt x/2 + for(int i=0; i<3; i++) + x_outg[i] = + x_ing[i] + + cross[i] + + rg[i]*inner / 2.; + } + else + { + // Non-small rotation. This is the normal path. Note that this path is + // still valid even if th ~ 180deg + + const val_withgrad_t th = th2.sqrt(); + const vec_withgrad_t sc = th.sincos(); + + for(int i=0; i<3; i++) + x_outg[i] = + x_ing[i]*sc.v[1] + + cross[i] * sc.v[0]/th + + rg[i] * inner * (val_withgrad_t(1.) - sc.v[1]) / th2; + } +} + +extern "C" +void mrcal_rotate_point_r_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_r, // (3,3) array. May be NULL + int J_r_stride0, // in bytes. <= 0 means "contiguous" + int J_r_stride1, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r, // (3,) array. May be NULL + int r_stride0, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // rotation in the opposite + // direction. J_r corresponds + // to the input r + ) +{ + init_stride_1D(x_out, 3); + init_stride_2D(J_r, 3,3); + init_stride_2D(J_x, 3,3); + init_stride_1D(r, 3); + init_stride_1D(x_in, 3); + + if(J_r == NULL && J_x == NULL) + { + vec_withgrad_t<0, 3> rg (r, -1, r_stride0); + vec_withgrad_t<0, 3> x_ing(x_in, -1, x_in_stride0); + vec_withgrad_t<0, 3> x_outg; + rotate_point_r_core<0>(x_outg.v, + rg.v, x_ing.v, + inverted); + x_outg.extract_value(x_out, x_out_stride0); + } + else if(J_r != NULL && J_x == NULL) + { + vec_withgrad_t<3, 3> rg (r, 0, r_stride0); + vec_withgrad_t<3, 3> x_ing(x_in, -1, x_in_stride0); + vec_withgrad_t<3, 3> x_outg; + rotate_point_r_core<3>(x_outg.v, + rg.v, x_ing.v, + inverted); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_r, 0, 3, 0, J_r_stride0, J_r_stride1); + } + else if(J_r == NULL && J_x != NULL) + { + vec_withgrad_t<3, 3> rg (r, -1, r_stride0); + vec_withgrad_t<3, 3> x_ing(x_in, 0, x_in_stride0); + vec_withgrad_t<3, 3> x_outg; + rotate_point_r_core<3>(x_outg.v, + rg.v, x_ing.v, + inverted); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_x, 0, 3, 0, J_x_stride0,J_x_stride1); + } + else + { + vec_withgrad_t<6, 3> rg (r, 0, r_stride0); + vec_withgrad_t<6, 3> x_ing(x_in, 3, x_in_stride0); + vec_withgrad_t<6, 3> x_outg; + rotate_point_r_core<6>(x_outg.v, + rg.v, x_ing.v, + inverted); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_r, 0, 3, 0, J_r_stride0, J_r_stride1); + x_outg.extract_grad (J_x, 3, 3, 0, J_x_stride0, J_x_stride1); + } +} + +extern "C" +void mrcal_transform_point_rt_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_rt, // (3,6) array. May be NULL + int J_rt_stride0, // in bytes. <= 0 means "contiguous" + int J_rt_stride1, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt, // (6,) array. May be NULL + int rt_stride0, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply the + // transformation in the + // opposite direction. + // J_rt corresponds to + // the input rt + ) +{ + if(!inverted) + { + init_stride_1D(x_out, 3); + init_stride_2D(J_rt, 3,6); + // init_stride_2D(J_x, 3,3 ); + init_stride_1D(rt, 6 ); + // init_stride_1D(x_in, 3 ); + + // to make in-place operations work + double t[3] = { P1(rt, 3), + P1(rt, 4), + P1(rt, 5) }; + // I want rotate(x) + t + // First rotate(x) + mrcal_rotate_point_r_full(x_out, x_out_stride0, + J_rt, J_rt_stride0, J_rt_stride1, + J_x, J_x_stride0, J_x_stride1, + rt, rt_stride0, + x_in, x_in_stride0, false); + + // And now +t. The J_r, J_x gradients are unaffected. J_t is identity + for(int i=0; i<3; i++) + P1(x_out,i) += t[i]; + if(J_rt) + mrcal_identity_R_full(&P2(J_rt,0,3), J_rt_stride0, J_rt_stride1); + } + else + { + // I use the special-case rx_minus_rt() to efficiently rotate both x and + // t by the same r + init_stride_1D(x_out, 3); + init_stride_2D(J_rt, 3,6); + init_stride_2D(J_x, 3,3 ); + init_stride_1D(rt, 6 ); + init_stride_1D(x_in, 3 ); + + if(J_rt == NULL && J_x == NULL) + { + vec_withgrad_t<0, 3> x_minus_t(x_in, -1, x_in_stride0); + x_minus_t -= vec_withgrad_t<0, 3>(&P1(rt,3), -1, rt_stride0); + + vec_withgrad_t<0, 3> rg (&rt[0], -1, rt_stride0); + vec_withgrad_t<0, 3> x_outg; + rotate_point_r_core<0>(x_outg.v, + rg.v, x_minus_t.v, + true); + x_outg.extract_value(x_out, x_out_stride0); + } + else if(J_rt != NULL && J_x == NULL) + { + vec_withgrad_t<6, 3> x_minus_t(x_in, -1, x_in_stride0); + x_minus_t -= vec_withgrad_t<6, 3>(&P1(rt,3), 3, rt_stride0); + + vec_withgrad_t<6, 3> rg (&rt[0], 0, rt_stride0); + vec_withgrad_t<6, 3> x_outg; + rotate_point_r_core<6>(x_outg.v, + rg.v, x_minus_t.v, + true); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_rt, 0, 3, 0, J_rt_stride0, J_rt_stride1); + x_outg.extract_grad (&P2(J_rt,0,3), 3, 3, 0, J_rt_stride0, J_rt_stride1); + } + else if(J_rt == NULL && J_x != NULL) + { + vec_withgrad_t<3, 3> x_minus_t(x_in, 0, x_in_stride0); + x_minus_t -= vec_withgrad_t<3, 3>(&P1(rt,3),-1, rt_stride0); + + vec_withgrad_t<3, 3> rg (&rt[0], -1, rt_stride0); + vec_withgrad_t<3, 3> x_outg; + rotate_point_r_core<3>(x_outg.v, + rg.v, x_minus_t.v, + true); + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_x, 0, 3, 0, J_x_stride0, J_x_stride1); + } + else + { + vec_withgrad_t<9, 3> x_minus_t(x_in, 3, x_in_stride0); + x_minus_t -= vec_withgrad_t<9, 3>(&P1(rt,3), 6, rt_stride0); + + vec_withgrad_t<9, 3> rg (&rt[0], 0, rt_stride0); + vec_withgrad_t<9, 3> x_outg; + + + rotate_point_r_core<9>(x_outg.v, + rg.v, x_minus_t.v, + true); + + x_outg.extract_value(x_out, x_out_stride0); + x_outg.extract_grad (J_rt, 0, 3, 0, J_rt_stride0, J_rt_stride1); + x_outg.extract_grad (&P2(J_rt,0,3), 6, 3, 0, J_rt_stride0, J_rt_stride1); + x_outg.extract_grad (J_x, 3, 3, 0, J_x_stride0, J_x_stride1); + } + } +} + + +template +static void +compose_r_core(// output + vec_withgrad_t* r, + + // inputs + 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 + + 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.; + + 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.) + { + // A ~ 0. I simplify + // + // cosC ~ + // + cosB + // - A*sinB * inner(r0,r1) / 4AB + // sinC r01 / 2C ~ + // + A*cosB* r0 / 2A + // + sinB * r1 / 2B + // + A*sinB * cross(r0,r1) / 4AB + // + // I have C = B + dB where dB ~ 0, so + // + // cosC ~ cos(B + dB) ~ cosB - dB sinB + // -> dB = A * inner(r0,r1) / 4AB = + // inner(r0,r1) / 4B + // -> C = B + inner(r0,r1) / 4B + // + // Now let's look at the axis expression. Assuming A ~ 0 + // + // sinC r01 / 2C ~ + // + A*cosB r0 / 2A + // + sinB r1 / 2B + // + A*sinB * cross(r0,r1) / 4AB + // -> + // sinC/C * r01 ~ + // + cosB r0 + // + sinB r1 / B + // + sinB * cross(r0,r1) / 2B + // + // I linearize the left-hand side: + // + // sinC/C = sin(B+dB)/(B+dB) ~ + // sinB/B + d( sinB/B )/dB dB = + // sinB/B + dB (B cosB - sinB) / B^2 + // + // So + // + // (sinB/B + dB (B cosB - sinB) / B^2) r01 ~ + // + cosB r0 + // + sinB r1 / B + // + sinB * cross(r0,r1) / 2B + // -> + // (sinB + dB (B cosB - sinB) / B) r01 ~ + // + cosB*B r0 + // + sinB r1 + // + sinB * cross(r0,r1) / 2 + // -> + // sinB (r01 - r1) + dB (B cosB - sinB) / B r01 ~ + // + cosB*B r0 + // + sinB * cross(r0,r1) / 2 + // + // I want to find the perturbation to give me r01 ~ r1 + deltar -> + // + // sinB deltar + dB (B cosB - sinB) / B (r1 + deltar) ~ + // + cosB*B r0 + // + sinB * cross(r0,r1) / 2 + // + // All terms here are linear or quadratic in r0. For tiny r0, I can + // ignore the quadratic terms: + // + // sinB deltar + dB (B cosB - sinB) / B r1 ~ + // + cosB*B r0 + // + sinB * cross(r0,r1) / 2 + // -> + // deltar ~ + // - dB (B/tanB - 1) / B r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + // + // I substitute in the dB from above, and I simplify: + // + // deltar ~ + // - inner(r0,r1) / 4B (B/tanB - 1) / B r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + // = + // - inner(r0,r1) (B/tanB - 1) / 4B^2 r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + // + // So + // + // r01 = r1 + // - inner(r0,r1) (B/tanB - 1) / 4B^2 r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + if(B.x < eps) + { + // what if B is ALSO near 0? I simplify further + // + // lim(B->0) (B/tanB) = lim( 1 / sec^2 B) = 1. + // lim(B->0) d(B/tanB)/dB = 0 + // + // (B/tanB - 1) / 4B^2 = + // (B - tanB) / (4 B^2 tanB) + // lim(B->0) = 0 + // lim(B->0) d/dB = 0 + // + // So + // r01 = r1 + // + r0 + // + cross(r0,r1) / 2 + // + // Here I have linear and quadratic terms. With my tiny numbers, the + // quadratic terms can be ignored, so simply + // + // r01 = r0 + r1 + *r = *r0 + *r1; + return; + } + + const val_withgrad_t& B_over_tanB = B / B.tan(); + for(int i=0; i<3; i++) + (*r)[i] = + (*r1)[i] * (val_withgrad_t(1.0) + - inner * (B_over_tanB - 1.) / (B*B*4.)) + + (*r0)[i] * B_over_tanB + + cross[i] / 2.; + return; + } + else if(B.x < eps) + { + // B ~ 0. I simplify + // + // cosC = + // cosA - + // sinA*B * inner(r0,r1) / 4AB + // sinC r01 / 2C = + // sinA*r0 / 2A + + // cosA*B*r1 / 2B + + // sinA*B * cross(r0,r1) / 4AB + // + // I have C = A + dA where dA ~ 0, so + // + // cosC ~ cos(A + dA) ~ cosA - dA sinA + // -> dA = B * inner(r0,r1) / 4AB = + // = inner(r0,r1) / 4A + // -> C = A + inner(r0,r1) / 4A + // + // Now let's look at the axis expression. Assuming B ~ 0 + // + // sinC r01 / 2C = + // + sinA*r0 / 2A + // + cosA*B*r1 / 2B + // + sinA*B * cross(r0,r1) / 4AB + // -> + // sinC/C r01 = + // + sinA*r0 / A + // + cosA*r1 + // + sinA * cross(r0,r1) / 2A + // + // I linearize the left-hand side: + // + // sinC/C = sin(A+dA)/(A+dA) ~ + // sinA/A + d( sinA/A )/dA dA = + // sinA/A + dA (A cosA - sinA) / A^2 + // + // So + // + // (sinA/A + dA (A cosA - sinA) / A^2) r01 ~ + // + sinA*r0 / A + // + cosA*r1 + // + sinA * cross(r0,r1) / 2A + // -> + // (sinA + dA (A cosA - sinA) / A) r01 ~ + // + sinA*r0 + // + cosA*r1*A + // + sinA * cross(r0,r1) / 2 + // -> + // sinA (r01 - r0) + dA (A cosA - sinA) / A r01 ~ + // + cosA*A r1 + // + sinA * cross(r0,r1) / 2 + // + // + // I want to find the perturbation to give me r01 ~ r0 + deltar -> + // + // sinA deltar + dA (A cosA - sinA) / A (r0 + deltar) ~ + // + cosA*A r1 + // + sinA * cross(r0,r1) / 2 + // + // All terms here are linear or quadratic in r1. For tiny r1, I can + // ignore the quadratic terms: + // + // sinA deltar + dA (A cosA - sinA) / A r0 ~ + // + cosA*A r1 + // + sinA * cross(r0,r1) / 2 + // -> + // deltar ~ + // - dA (A/tanA - 1) / A r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + // + // I substitute in the dA from above, and I simplify: + // + // deltar ~ + // - inner(r0,r1) / 4A (A/tanA - 1) / A r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + // = + // - inner(r0,r1) (A/tanA - 1) / 4A^2 r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + // + // So + // + // r01 = r0 + // - inner(r0,r1) (A/tanA - 1) / 4A^2 r0 + // + A/tanA r1 + // + cross(r0,r1) / 2 + + // I don't have an if(A.x < eps){} case here; this is handled in + // the above if() branch + + const val_withgrad_t& A_over_tanA = A / A.tan(); + for(int i=0; i<3; i++) + (*r)[i] = + (*r0)[i] * (val_withgrad_t(1.0) + - inner * (A_over_tanA - 1.) / (A*A*4.)) + + (*r1)[i] * A_over_tanA + + cross[i] / 2.; + return; + } + + const vec_withgrad_t sincosA = A.sincos(); + const vec_withgrad_t sincosB = B.sincos(); + + const val_withgrad_t& sinA = sincosA.v[0]; + const val_withgrad_t& cosA = sincosA.v[1]; + const val_withgrad_t& sinB = sincosB.v[0]; + const val_withgrad_t& cosB = sincosB.v[1]; + + const val_withgrad_t& sinA_over_A = A.sinx_over_x(sinA); + const val_withgrad_t& sinB_over_B = B.sinx_over_x(sinB); + + val_withgrad_t cosC = + 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(); + + 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; + } +} + +extern "C" +void mrcal_compose_r_full( // output + double* r_out, // (3,) array + int r_out_stride0, // in bytes. <= 0 means "contiguous" + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + 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" + const double* r_1, // (3,) array + int r_1_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(r_out, 3); + init_stride_2D(dr_dr0, 3,3); + init_stride_2D(dr_dr1, 3,3); + init_stride_1D(r_0, 3); + init_stride_1D(r_1, 3); + + if(dr_dr0 == NULL && dr_dr1 == NULL) + { + // no gradients + vec_withgrad_t<0, 3> r0g, r1g; + + r0g.init_vars(r_0, + 0, 3, -1, + r_0_stride0); + r1g.init_vars(r_1, + 0, 3, -1, + r_1_stride0); + + vec_withgrad_t<0, 3> r01g; + compose_r_core<0>( &r01g, + &r0g, &r1g ); + + r01g.extract_value(r_out, r_out_stride0, + 0, 3); + } + else if(dr_dr0 != NULL && dr_dr1 == NULL) + { + // r0 gradient only + vec_withgrad_t<3, 3> r0g, r1g; + + r0g.init_vars(r_0, + 0, 3, 0, + r_0_stride0); + r1g.init_vars(r_1, + 0, 3, -1, + r_1_stride0); + + vec_withgrad_t<3, 3> r01g; + compose_r_core<3>( &r01g, + &r0g, &r1g ); + + r01g.extract_value(r_out, r_out_stride0, + 0, 3); + r01g.extract_grad(dr_dr0, + 0,3, + 0, + dr_dr0_stride0, dr_dr0_stride1, + 3); + } + else if(dr_dr0 == NULL && dr_dr1 != NULL) + { + // r1 gradient only + vec_withgrad_t<3, 3> r0g, r1g; + + r0g.init_vars(r_0, + 0, 3, -1, + r_0_stride0); + r1g.init_vars(r_1, + 0, 3, 0, + r_1_stride0); + + vec_withgrad_t<3, 3> r01g; + compose_r_core<3>( &r01g, + &r0g, &r1g ); + + r01g.extract_value(r_out, r_out_stride0, + 0, 3); + r01g.extract_grad(dr_dr1, + 0,3, + 0, + dr_dr1_stride0, dr_dr1_stride1, + 3); + } + else + { + // r0 AND r1 gradients + vec_withgrad_t<6, 3> r0g, r1g; + + r0g.init_vars(r_0, + 0, 3, 0, + r_0_stride0); + r1g.init_vars(r_1, + 0, 3, 3, + r_1_stride0); + + vec_withgrad_t<6, 3> r01g; + compose_r_core<6>( &r01g, + &r0g, &r1g ); + + r01g.extract_value(r_out, r_out_stride0, + 0, 3); + r01g.extract_grad(dr_dr0, + 0,3, + 0, + dr_dr0_stride0, dr_dr0_stride1, + 3); + r01g.extract_grad(dr_dr1, + 3,3, + 0, + dr_dr1_stride0, dr_dr1_stride1, + 3); + } +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c b/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c new file mode 100644 index 00000000000..59f77384771 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils.c @@ -0,0 +1,1129 @@ +// 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 + +// Apparently I need this in MSVC to get constants +#define _USE_MATH_DEFINES + +#include +#include +#include + +#include "poseutils.h" +#include "strides.h" +#include "minimath/minimath.h" + +// All arrays stored in row-major order +// +// I have two different representations of pose transformations: +// +// - Rt is a concatenated (4,3) array: Rt = nps.glue(R,t, axis=-2). The +// transformation is R*x+t +// +// - rt is a concatenated (6) array: rt = nps.glue(r,t, axis=-1). The +// transformation is R*x+t where R = R_from_r(r) + + +// row vectors: vout = matmult(v,Mt) +// equivalent col vector expression: vout = matmult(M,v) +#define mul_vec3_gen33t_vout_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1, \ + scale) \ + do { \ + /* needed for in-place operations */ \ + double outcopy[3] = { \ + scale * \ + (_P2(Mt,Mt_stride0,Mt_stride1,0,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,2)*_P1(v,v_stride0,2) ), \ + scale * \ + (_P2(Mt,Mt_stride0,Mt_stride1,1,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,2)*_P1(v,v_stride0,2) ), \ + scale * \ + (_P2(Mt,Mt_stride0,Mt_stride1,2,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,2)*_P1(v,v_stride0,2) ) }; \ + _P1(vout,vout_stride0,0) = outcopy[0]; \ + _P1(vout,vout_stride0,1) = outcopy[1]; \ + _P1(vout,vout_stride0,2) = outcopy[2]; \ + } while(0) +#define mul_vec3_gen33t_vout_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1) \ + mul_vec3_gen33t_vout_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1, 1.0) +// row vectors: vout = scale*matmult(v,M) +#define mul_vec3_gen33_vout_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + M, M_stride0, M_stride1, \ + scale) \ + do { \ + /* needed for in-place operations */ \ + double outcopy[3] = { \ + scale * \ + (_P2(M,M_stride0,M_stride1,0,0)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,0)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,0)*_P1(v,v_stride0,2)), \ + scale * \ + (_P2(M,M_stride0,M_stride1,0,1)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,1)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,1)*_P1(v,v_stride0,2)), \ + scale * \ + (_P2(M,M_stride0,M_stride1,0,2)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,2)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,2)*_P1(v,v_stride0,2)) }; \ + _P1(vout,vout_stride0,0) = outcopy[0]; \ + _P1(vout,vout_stride0,1) = outcopy[1]; \ + _P1(vout,vout_stride0,2) = outcopy[2]; \ + } while(0) +#define mul_vec3_gen33_vout_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1) \ + mul_vec3_gen33_vout_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1, 1.0) + +// row vectors: vout = matmult(v,Mt) +// equivalent col vector expression: vout = matmult(M,v) +#define mul_vec3_gen33t_vaccum_full(vout, vout_stride0, \ + v, v_stride0, \ + Mt, Mt_stride0, Mt_stride1) \ + do { \ + /* needed for in-place operations */ \ + double outcopy[3] = { \ + _P1(vout,vout_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,0,2)*_P1(v,v_stride0,2), \ + _P1(vout,vout_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,1,2)*_P1(v,v_stride0,2), \ + _P1(vout,vout_stride0,2) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,0)*_P1(v,v_stride0,0) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,1)*_P1(v,v_stride0,1) + \ + _P2(Mt,Mt_stride0,Mt_stride1,2,2)*_P1(v,v_stride0,2) }; \ + _P1(vout,vout_stride0,0) = outcopy[0]; \ + _P1(vout,vout_stride0,1) = outcopy[1]; \ + _P1(vout,vout_stride0,2) = outcopy[2]; \ + } while(0) + +// row vectors: vout = scale*matmult(v,M) +#define mul_vec3_gen33_vaccum_scaled_full(vout, vout_stride0, \ + v, v_stride0, \ + M, M_stride0, M_stride1, \ + scale) \ + do { \ + /* needed for in-place operations */ \ + double outcopy[3] = { \ + _P1(vout,vout_stride0,0) + scale * \ + (_P2(M,M_stride0,M_stride1,0,0)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,0)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,0)*_P1(v,v_stride0,2)), \ + _P1(vout,vout_stride0,1) + scale * \ + (_P2(M,M_stride0,M_stride1,0,1)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,1)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,1)*_P1(v,v_stride0,2)), \ + _P1(vout,vout_stride0,2) + scale * \ + (_P2(M,M_stride0,M_stride1,0,2)*_P1(v,v_stride0,0) + \ + _P2(M,M_stride0,M_stride1,1,2)*_P1(v,v_stride0,1) + \ + _P2(M,M_stride0,M_stride1,2,2)*_P1(v,v_stride0,2)) }; \ + _P1(vout,vout_stride0,0) = outcopy[0]; \ + _P1(vout,vout_stride0,1) = outcopy[1]; \ + _P1(vout,vout_stride0,2) = outcopy[2]; \ + } while(0) + +// multiply two (3,3) matrices +static inline +void mul_gen33_gen33_vout_full(// output + double* m0m1, + int m0m1_stride0, int m0m1_stride1, + + // input + const double* m0, + int m0_stride0, int m0_stride1, + const double* m1, + int m1_stride0, int m1_stride1) +{ + /* needed for in-place operations */ + double outcopy2[9]; + + for(int i=0; i<3; i++) + // one row at a time + mul_vec3_gen33_vout_scaled_full(&outcopy2[i*3], sizeof(outcopy2[0]), + &_P2(m0 , m0_stride0, m0_stride1, i,0), m0_stride1, + m1, m1_stride0, m1_stride1, + 1.0); + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(m0m1, i,j) = outcopy2[3*i+j]; +} + +static inline +double inner3(const double* restrict a, + const double* restrict b) +{ + double s = 0.0; + for (int i=0; i<3; i++) s += a[i]*b[i]; + return s; +} + + + + +// Make an identity rotation or transformation +void mrcal_identity_R_full(double* R, // (3,3) array + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(R, 3,3); + P2(R, 0,0) = 1.0; P2(R, 0,1) = 0.0; P2(R, 0,2) = 0.0; + P2(R, 1,0) = 0.0; P2(R, 1,1) = 1.0; P2(R, 1,2) = 0.0; + P2(R, 2,0) = 0.0; P2(R, 2,1) = 0.0; P2(R, 2,2) = 1.0; +} +void mrcal_identity_r_full(double* r, // (3,) array + int r_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(r, 3); + P1(r, 0) = 0.0; P1(r, 1) = 0.0; P1(r, 2) = 0.0; +} +void mrcal_identity_Rt_full(double* Rt, // (4,3) array + int Rt_stride0, // in bytes. <= 0 means "contiguous" + int Rt_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(Rt, 4,3); + mrcal_identity_R_full(Rt, Rt_stride0, Rt_stride1); + for(int i=0; i<3; i++) P2(Rt, 3, i) = 0.0; +} +void mrcal_identity_rt_full(double* rt, // (6,) array + int rt_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(rt, 6); + mrcal_identity_r_full(rt, rt_stride0); + for(int i=0; i<3; i++) P1(rt, i+3) = 0.0; +} + +void mrcal_rotate_point_R_full( // output + double* x_out, // (3,) array + int x_out_stride0, // in bytes. <= 0 means "contiguous" + double* J_R, // (3,3,3) array. May be NULL + int J_R_stride0, // in bytes. <= 0 means "contiguous" + int J_R_stride1, // in bytes. <= 0 means "contiguous" + int J_R_stride2, // in bytes. <= 0 means "contiguous" + double* J_x, // (3,3) array. May be NULL + int J_x_stride0, // in bytes. <= 0 means "contiguous" + int J_x_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* R, // (3,3) array. May be NULL + int R_stride0, // in bytes. <= 0 means "contiguous" + int R_stride1, // in bytes. <= 0 means "contiguous" + const double* x_in, // (3,) array. May be NULL + int x_in_stride0, // in bytes. <= 0 means "contiguous" + + bool inverted // if true, I apply a + // rotation in the opposite + // direction. J_R corresponds + // to the input R + ) +{ + init_stride_1D(x_out, 3); + init_stride_3D(J_R, 3,3,3 ); + init_stride_2D(J_x, 3,3 ); + init_stride_2D(R, 3,3 ); + init_stride_1D(x_in, 3 ); + + if(inverted) + { + // transpose R + int tmp; + + tmp = R_stride0; + R_stride0 = R_stride1; + R_stride1 = tmp; + + tmp = J_R_stride1; + J_R_stride1 = J_R_stride2; + J_R_stride2 = tmp; + } + + if(J_R) + { + // out[i] = inner(R[i,:],in) + for(int i=0; i<3; i++) + { + int j=0; + for(; j a = R'b - R't +void mrcal_invert_Rt_full( // output + double* Rt_out, // (4,3) array + int Rt_out_stride0, // in bytes. <= 0 means "contiguous" + int Rt_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt_in, // (4,3) array + int Rt_in_stride0, // in bytes. <= 0 means "contiguous" + int Rt_in_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(Rt_out, 4,3); + init_stride_2D(Rt_in, 4,3); + + // transpose(R). Extra stuff to make in-place operations work + for(int i=0; i<3; i++) + P2(Rt_out,i,i) = P2(Rt_in,i,i); + for(int i=0; i<3; i++) + for(int j=i+1; j<3; j++) + { + double tmp = P2(Rt_in,i,j); + P2(Rt_out,i,j) = P2(Rt_in,j,i); + P2(Rt_out,j,i) = tmp; + } + + // -transpose(R)*t + mul_vec3_gen33t_vout_scaled_full(&P2(Rt_out,3,0), Rt_out_stride1, + &P2(Rt_in, 3,0), Rt_in_stride1, + Rt_out, Rt_out_stride0, Rt_out_stride1, + -1.0); +} + +// Invert an rt transformation +// +// b = rotate(a) + t -> a = invrotate(b) - invrotate(t) +// +// drout_drin is not returned: it is always -I +// drout_dtin is not returned: it is always 0 +void mrcal_invert_rt_full( // output + double* rt_out, // (6,) array + int rt_out_stride0, // in bytes. <= 0 means "contiguous" + double* dtout_drin, // (3,3) array + int dtout_drin_stride0, // in bytes. <= 0 means "contiguous" + int dtout_drin_stride1, // in bytes. <= 0 means "contiguous" + double* dtout_dtin, // (3,3) array + int dtout_dtin_stride0, // in bytes. <= 0 means "contiguous" + int dtout_dtin_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt_in, // (6,) array + int rt_in_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(rt_out, 6); + // init_stride_2D(dtout_drin, 3,3); + init_stride_2D(dtout_dtin, 3,3); + init_stride_1D(rt_in, 6); + + // r uses an angle-axis representation, so to undo a rotation r, I can apply + // a rotation -r (same axis, equal and opposite angle) + for(int i=0; i<3; i++) + P1(rt_out,i) = -P1(rt_in,i); + + mrcal_rotate_point_r_full( &P1(rt_out,3), rt_out_stride0, + dtout_drin, dtout_drin_stride0, dtout_drin_stride1, + dtout_dtin, dtout_dtin_stride0, dtout_dtin_stride1, + + // input + rt_out, rt_out_stride0, + &P1(rt_in,3), rt_in_stride0, + false); + for(int i=0; i<3; i++) + P1(rt_out,3+i) *= -1.; + + if(dtout_dtin) + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(dtout_dtin,i,j) *= -1.; +} + + +// Compose two Rt transformations +// R0*(R1*x + t1) + t0 = +// (R0*R1)*x + R0*t1+t0 +void mrcal_compose_Rt_full( // output + double* Rt_out, // (4,3) array + int Rt_out_stride0, // in bytes. <= 0 means "contiguous" + int Rt_out_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* Rt_0, // (4,3) array + int Rt_0_stride0, // in bytes. <= 0 means "contiguous" + int Rt_0_stride1, // in bytes. <= 0 means "contiguous" + const double* Rt_1, // (4,3) array + int Rt_1_stride0, // in bytes. <= 0 means "contiguous" + int Rt_1_stride1 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(Rt_out, 4,3); + init_stride_2D(Rt_0, 4,3); + init_stride_2D(Rt_1, 4,3); + + // for in-place operation + double t0[] = { P2(Rt_0,3,0), + P2(Rt_0,3,1), + P2(Rt_0,3,2) }; + + // t <- R0*t1 + mul_vec3_gen33t_vout_full(&P2(Rt_out,3,0), Rt_out_stride1, + &P2(Rt_1, 3,0), Rt_1_stride1, + Rt_0, Rt_0_stride0, Rt_0_stride1); + + // R <- R0*R1 + mul_gen33_gen33_vout_full( Rt_out, Rt_out_stride0, Rt_out_stride1, + Rt_0, Rt_0_stride0, Rt_0_stride1, + Rt_1, Rt_1_stride0, Rt_1_stride1 ); + + // t <- R0*t1+t0 + for(int i=0; i<3; i++) + P2(Rt_out,3,i) += t0[i]; +} + +// Compose two rt transformations. It is assumed that we're getting no gradients +// at all or we're getting ALL the gradients: only dr_r0 is checked for NULL +// +// dr_dt0 is not returned: it is always 0 +// dr_dt1 is not returned: it is always 0 +// dt_dr1 is not returned: it is always 0 +// dt_dt0 is not returned: it is always the identity matrix +void mrcal_compose_rt_full( // output + double* rt_out, // (6,) array + int rt_out_stride0, // in bytes. <= 0 means "contiguous" + double* dr_r0, // (3,3) array; may be NULL + int dr_r0_stride0, // in bytes. <= 0 means "contiguous" + int dr_r0_stride1, // in bytes. <= 0 means "contiguous" + double* dr_r1, // (3,3) array; may be NULL + int dr_r1_stride0, // in bytes. <= 0 means "contiguous" + int dr_r1_stride1, // in bytes. <= 0 means "contiguous" + double* dt_r0, // (3,3) array; may be NULL + int dt_r0_stride0, // in bytes. <= 0 means "contiguous" + int dt_r0_stride1, // in bytes. <= 0 means "contiguous" + double* dt_t1, // (3,3) array; may be NULL + int dt_t1_stride0, // in bytes. <= 0 means "contiguous" + int dt_t1_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* rt_0, // (6,) array + int rt_0_stride0, // in bytes. <= 0 means "contiguous" + const double* rt_1, // (6,) array + int rt_1_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_1D(rt_out, 6); + init_stride_2D(dr_r0, 3,3); + init_stride_2D(dr_r1, 3,3); + init_stride_2D(dt_r0, 3,3); + init_stride_2D(dt_t1, 3,3); + init_stride_1D(rt_0, 6); + init_stride_1D(rt_1, 6); + + // r0 (r1 x + t1) + t0 = r0 r1 x + r0 t1 + t0 + // -> I want (r0 r1, r0 t1 + t0) + + + // to make in-place operation work + double rt0[6]; + for(int i=0; i<6; i++) + rt0[i] = P1(rt_0, i); + + // Compute r01 + mrcal_compose_r_full( rt_out, rt_out_stride0, + dr_r0, dr_r0_stride0, dr_r0_stride1, + dr_r1, dr_r1_stride0, dr_r1_stride1, + + rt_0, rt_0_stride0, + rt_1, rt_1_stride0); + + + // t01 <- r0 t1 + mrcal_rotate_point_r_full( &P1(rt_out,3), rt_out_stride0, + dt_r0, dt_r0_stride0, dt_r0_stride1, + dt_t1, dt_t1_stride0, dt_t1_stride1, + + rt0, -1, + &P1(rt_1,3), rt_1_stride0, + + false ); + // t01 <- r0 t1 + t0 + for(int i=0; i<3; i++) + P1(rt_out,3+i) += rt0[3+i]; +} + +void mrcal_compose_r_tinyr0_gradientr0_full( // output + double* dr_dr0, // (3,3) array; may be NULL + int dr_dr0_stride0, // in bytes. <= 0 means "contiguous" + int dr_dr0_stride1, // in bytes. <= 0 means "contiguous" + + // input + const double* r_1, // (3,) array + int r_1_stride0 // in bytes. <= 0 means "contiguous" + ) +{ + init_stride_2D(dr_dr0, 3, 3); + init_stride_1D(r_1, 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_r1 = 0.0; + for(int i=0; i<3; i++) + norm2_r1 += P1(r_1,i)*P1(r_1,i); + + if(norm2_r1 < 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_dr0,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 = r1 + // - inner(r0,r1) (B/tanB - 1) / 4B^2 r1 + // + B/tanB r0 + // + cross(r0,r1) / 2 + // + // I differentiate: + // + // dr01/dr0 = + // - outer(r1,r1) (B/tanB - 1) / 4B^2 + // + B/tanB I + // - skew_symmetric(r1) / 2 + double B = sqrt(norm2_r1) / 2.; + double B_over_tanB = B / tan(B); + + for(int i=0; i<3; i++) + for(int j=0; j<3; j++) + P2(dr_dr0,i,j) = + - P1(r_1,i)*P1(r_1,j) * (B_over_tanB - 1.) / (4.*B*B); + for(int i=0; i<3; i++) + P2(dr_dr0,i,i) += + B_over_tanB; + + P2(dr_dr0,0,1) -= -P1(r_1,2)/2.; + P2(dr_dr0,0,2) -= P1(r_1,1)/2.; + P2(dr_dr0,1,0) -= P1(r_1,2)/2.; + P2(dr_dr0,1,2) -= -P1(r_1,0)/2.; + 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]; + } + } +} + +// Compute a non-unique rotation to map a given vector to [0,0,1] +// See docstring for mrcal.R_aligned_to_vector() for details +void mrcal_R_aligned_to_vector(// out + double* R, + // in + const double* v) +{ + double magv = sqrt(v[0]*v[0] + + v[1]*v[1] + + v[2]*v[2]); + + double* x = &R[0*3]; + double* y = &R[1*3]; + double* z = &R[2*3]; + + for(int i=0; i<3; i++) + { + x[i] = 0.0; + z[i] = v[i]/magv; + } + + double inner_x_z = 0.; + if(fabs(z[0]) < .9) + { + x[0] = 1.; + inner_x_z = z[0]; + } + else + { + x[1] = 1.; + inner_x_z = z[1]; + } + + for(int i=0; i<3; i++) + x[i] -= inner_x_z*z[i]; + + double magx = sqrt(x[0]*x[0] + + x[1]*x[1] + + x[2]*x[2]); + + for(int i=0; i<3; i++) + x[i] /= magx; + + // y = cross(z,x); + y[0] = z[1]*x[2] - z[2]*x[1]; + y[1] = z[2]*x[0] - z[0]*x[2]; + y[2] = z[0]*x[1] - z[1]*x[0]; +} diff --git a/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp b/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp new file mode 100644 index 00000000000..ca8ea012f4c --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp @@ -0,0 +1,1032 @@ +// 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 "autodiff.hh" + +extern "C" { +#include "triangulation.h" +} + +template +static +bool +triangulate_assume_intersect( // output + vec_withgrad_t& m, + + // inputs. camera-0 coordinates + const vec_withgrad_t& v0, + const vec_withgrad_t& v1, + const vec_withgrad_t& t01) +{ + // I take two 3D rays that are assumed to intersect, and return the + // intersection point. Results are undefined if these rays actually + // don't intersect + + // Each pixel observation represents a ray in 3D: + // + // k0 v0 = t01 + k1 v1 + // + // t01 = [v0 -v1] k + // + // This is over-determined: k has 2DOF, but I have 3 equations. I know that + // the vectors intersect, so I can use 2 axes only, which makes the problem + // uniquely determined. Let's pick the 2 axes to use. The "forward" + // direction (z) is dominant, so let's use that. For the second axis, let's + // use whichever is best numerically: biggest abs(det), so that I divide by + // something as far away from 0 as possible. I have + // + double fabs_det_xz = fabs(-v0.v[0].x*v1.v[2].x + v0.v[2].x*v1.v[0].x); + double fabs_det_yz = fabs(-v0.v[1].x*v1.v[2].x + v0.v[2].x*v1.v[1].x); + + // If using xz, I have: + // + // k = 1/(-v0[0]*v1[2] + v0[2]*v1[0]) * [-v1[2] v1[0] ] t01 + // [-v0[2] v0[0] ] + // [0] -> [1] if using yz + val_withgrad_t k0; + if(fabs_det_xz > fabs_det_yz) + { + // xz + if(fabs_det_xz <= 1e-10) + return false; + + val_withgrad_t det = v1.v[0]*v0.v[2] - v0.v[0]*v1.v[2]; + k0 = (t01.v[2]*v1.v[0] - t01.v[0]*v1.v[2]) / det; + if(k0.x <= 0.0) + return false; + bool k1_negative = (t01.v[2].x*v0.v[0].x > t01.v[0].x*v0.v[2].x) ^ (det.x > 0); + if(k1_negative) + return false; + +#if 0 + val_withgrad_t k1 = (t01.v[2]*v0.v[0] - t01.v[0]*v0.v[2]) / det; + vec_withgrad_t m2 = v1*k1 + t01; + m2 -= m; + double d2 = m2.v[0].x*m2.v[0].x + m2.v[1].x*m2.v[1].x + m2.v[2].x*m2.v[2].x; + fprintf(stderr, "diff: %f\n", d2); +#endif + } + else + { + // yz + if(fabs_det_yz <= 1e-10) + return false; + + val_withgrad_t det = v1.v[1]*v0.v[2] - v0.v[1]*v1.v[2]; + k0 = (t01.v[2]*v1.v[1] - t01.v[1]*v1.v[2]) / det; + if(k0.x <= 0.0) + return false; + bool k1_negative = (t01.v[2].x*v0.v[1].x > t01.v[1].x*v0.v[2].x) ^ (det.x > 0); + if(k1_negative) + return false; + +#if 0 + val_withgrad_t k1 = (t01.v[2]*v0.v[1] - t01.v[1]*v0.v[2]) / det; + vec_withgrad_t m2 = v1*k1 + t01; + m2 -= m; + double d2 = m2.v[1].x*m2.v[1].x + m2.v[1].x*m2.v[1].x + m2.v[2].x*m2.v[2].x; + fprintf(stderr, "diff: %f\n", d2); +#endif + } + + m = v0 * k0; + + return true; +} + + +// #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 +mrcal_triangulate_geometric(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_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) +{ + // This is the basic 3D-geometry routine. I find the point in 3D that + // minimizes the distance to each of the observation rays. This is simple, + // but not as accurate as we'd like. All the other methods have lower + // biases. See the Lee-Civera papers for details: + // + // Paper that compares all methods implemented here: + // "Triangulation: Why Optimize?", Seong Hun Lee and Javier Civera. + // https://arxiv.org/abs/1907.11917 + // + // Earlier paper that doesn't have mid2 or wmid2: + // "Closed-Form Optimal Two-View Triangulation Based on Angular Errors", + // Seong Hun Lee and Javier Civera. ICCV 2019. + // + // Each pixel observation represents a ray in 3D. The best + // estimate of the 3d position of the point being observed + // is the point nearest to both these rays + // + // Let's say I have a ray from the origin to v0, and another ray from t01 + // to v1 (v0 and v1 aren't necessarily normal). Let the nearest points on + // each ray be k0 and k1 along each ray respectively: E = norm2(t01 + k1*v1 + // - k0*v0): + // + // dE/dk0 = 0 = inner(t01 + k1*v1 - k0*v0, -v0) + // dE/dk1 = 0 = inner(t01 + k1*v1 - k0*v0, v1) + // + // -> t01.v0 + k1 v0.v1 = k0 v0.v0 + // -t01.v1 + k0 v0.v1 = k1 v1.v1 + // + // -> [ v0.v0 -v0.v1] [k0] = [ t01.v0] + // [ -v0.v1 v1.v1] [k1] = [-t01.v1] + // + // -> [k0] = 1/(v0.v0 v1.v1 -(v0.v1)**2) [ v1.v1 v0.v1][ t01.v0] + // [k1] [ v0.v1 v0.v0][-t01.v1] + // + // I return the midpoint: + // + // x = (k0 v0 + t01 + k1 v1)/2 + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + val_withgrad_t<9> dot_v0v0 = v0.norm2(); + val_withgrad_t<9> dot_v1v1 = v1.norm2(); + val_withgrad_t<9> dot_v0v1 = v0.dot(v1); + val_withgrad_t<9> dot_v0t = v0.dot(t01); + val_withgrad_t<9> dot_v1t = v1.dot(t01); + + val_withgrad_t<9> denom = dot_v0v0*dot_v1v1 - dot_v0v1*dot_v0v1; + if(-1e-10 <= denom.x && denom.x <= 1e-10) + return (mrcal_point3_t){0}; + + val_withgrad_t<9> denom_recip = val_withgrad_t<9>(1.)/denom; + val_withgrad_t<9> k0 = denom_recip * (dot_v1v1*dot_v0t - dot_v0v1*dot_v1t); + if(k0.x <= 0.0) return (mrcal_point3_t){0}; + val_withgrad_t<9> k1 = denom_recip * (dot_v0v1*dot_v0t - dot_v0v0*dot_v1t); + if(k1.x <= 0.0) return (mrcal_point3_t){0}; + + vec_withgrad_t<9,3> m = (v0*k0 + v1*k1 + t01) * 0.5; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3, 0, + 3*sizeof(double), sizeof(double), + 3); + +#if 0 + MSG("intersecting..."); + MSG("v0 = (%.20f,%.20f,%.20f)", v0[0].x,v0[1].x,v0[2].x); + MSG("t01 = (%.20f,%.20f,%.20f)", t01[0].x,t01[1].x,t01[2].x); + MSG("v1 = (%.20f,%.20f,%.20f)", v1[0].x,v1[1].x,v1[2].x); + MSG("intersection = (%.20f,%.20f,%.20f) dist %f", + m.v[0].x,m.v[1].x,m.v[2].x, + sqrt( m.dot(m).x)); +#endif + + return _m; +} + +// Minimize L2 pinhole reprojection error. Described in "Triangulation Made +// Easy", Peter Lindstrom, IEEE Conference on Computer Vision and Pattern +// Recognition, 2010. +extern "C" +mrcal_point3_t +mrcal_triangulate_lindstrom(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_dRt01, + + // inputs + + // not-necessarily normalized vectors in the LOCAL + // coordinate system. This is different from the other + // triangulation routines + const mrcal_point3_t* _v0_local, + const mrcal_point3_t* _v1_local, + const mrcal_point3_t* _Rt01) +{ + // This is an implementation of the algorithm described in "Triangulation + // Made Easy", Peter Lindstrom, IEEE Conference on Computer Vision and + // Pattern Recognition, 2010. A copy of this paper is available in this repo + // in docs/TriangulationLindstrom.pdf. The implementation here is the niter2 + // routine in Listing 3. There's a higher-level implemented-in-python test + // in analyses/triangulation.py + // + // A simpler, but less-accurate way of doing is lives in + // triangulate_direct() + + // I'm looking at wikipedia for the Essential matrix definition: + // + // https://en.wikipedia.org/wiki/Essential_matrix + // + // and at Lindstrom's paper. Note that THEY HAVE DIFFERENT DEFINITIONS OF E + // + // I stick to Lindstrom's convention here. He has (section 2, equation 3) + // + // E = cross(t) R + // transpose(x0) E x1 = 0 + // + // What are R and t? + // + // x0' cross(t) R x1 = 0 + // x0' cross(t) R (R10 x0 + t10) = 0 + // + // So Lindstrom has R = R01 -> + // + // x0' cross(t) R01 (R10 x0 + t10) = 0 + // x0' cross(t) (x0 + R01 t10) = 0 + // x0' cross(t) R01 t10 = 0 + // + // This holds if Lindstrom has R01 t10 = +- t + // + // Note that if x1 = R10 x0 + t10 then x0 = R01 x1 - R01 t10 + // + // So I let t = t01 + // + // Thus he's multiplying cross(t01) and R01: + // + // E = cross(t01) R01 + // = cross(t01) R10' + + // cross(t01) = np.array(((0, -t01[2], t01[1]), + // ( t01[2], 0, -t01[0]), + // (-t01[1], t01[0], 0))); + + vec_withgrad_t<18,3> v0 (_v0_local->xyz, 0); + vec_withgrad_t<18,3> v1 (_v1_local->xyz, 3); + vec_withgrad_t<18,9> R01(_Rt01 ->xyz, 6); + vec_withgrad_t<18,3> t01(_Rt01[3] .xyz, 15); + + val_withgrad_t<18> E[9] = { R01[6]*t01[1] - R01[3]*t01[2], + R01[7]*t01[1] - R01[4]*t01[2], + R01[8]*t01[1] - R01[5]*t01[2], + + R01[0]*t01[2] - R01[6]*t01[0], + R01[1]*t01[2] - R01[7]*t01[0], + R01[2]*t01[2] - R01[8]*t01[0], + + R01[3]*t01[0] - R01[0]*t01[1], + R01[4]*t01[0] - R01[1]*t01[1], + R01[5]*t01[0] - R01[2]*t01[1] }; + + // Paper says to rescale x0,x1 such that their last element is 1.0. + // I don't even store it + val_withgrad_t<18> x0[2] = { v0[0]/v0[2], v0[1]/v0[2] }; + val_withgrad_t<18> x1[2] = { v1[0]/v1[2], v1[1]/v1[2] }; + + // for debugging +#if 0 + { + fprintf(stderr, "E:\n"); + for(int i=0; i<3; i++) + fprintf(stderr, "%f %f %f\n", E[0 + 3*i].x, E[1 + 3*i].x, E[2 + 3*i].x); + double Ex1[3] = { E[0].x*x1[0].x + E[1].x*x1[1].x + E[2].x, + E[3].x*x1[0].x + E[4].x*x1[1].x + E[5].x, + E[6].x*x1[0].x + E[7].x*x1[1].x + E[8].x }; + double x0Ex1 = Ex1[0]*x0[0].x + Ex1[1]*x0[1].x + Ex1[2]; + fprintf(stderr, "conj before: %f\n", x0Ex1); + } +#endif + + // Now I implement the algorithm. x0 here is x in the paper; x1 here + // is x' in the paper + + // Step 1. n = nps.matmult(x1, nps.transpose(E))[:2] + val_withgrad_t<18> n[2]; + n[0] = E[0]*x1[0] + E[1]*x1[1] + E[2]; + n[1] = E[3]*x1[0] + E[4]*x1[1] + E[5]; + + + // Step 2. nn = nps.matmult(x0, E)[:2] + val_withgrad_t<18> nn[2]; + nn[0] = E[0]*x0[0] + E[3]*x0[1] + E[6]; + nn[1] = E[1]*x0[0] + E[4]*x0[1] + E[7]; + + // Step 3. a = nps.matmult( n, E[:2,:2], nps.transpose(nn) ).ravel() + val_withgrad_t<18> a = + n[0]*E[0]*nn[0] + + n[0]*E[1]*nn[1] + + n[1]*E[3]*nn[0] + + n[1]*E[4]*nn[1]; + + // Step 4. b = 0.5*( nps.inner(n,n) + nps.inner(nn,nn) ) + val_withgrad_t<18> b = (n [0]*n [0] + n [1]*n [1] + + nn[0]*nn[0] + nn[1]*nn[1]) * 0.5; + + // Step 5. c = nps.matmult(x0, E, nps.transpose(x1)).ravel() + val_withgrad_t<18> n_2 = + E[6]*x1[0] + + E[7]*x1[1] + + E[8]; + val_withgrad_t<18> c = + n[0]*x0[0] + + n[1]*x0[1] + + n_2; + + // Step 6. d = np.sqrt( b*b - a*c ) + val_withgrad_t<18> d = (b*b - a*c).sqrt(); + + // Step 7. l = c / (b+d) + val_withgrad_t<18> l = c / (b + d); + + // Step 8. dx = l*n + val_withgrad_t<18> dx[2] = { l * n[0], l * n[1] }; + + // Step 9. dxx = l*nn + val_withgrad_t<18> dxx[2] = { l * nn[0], l * nn[1] }; + + // Step 10. n -= nps.matmult(dxx, nps.transpose(E[:2,:2])) + n[0] = n[0] - E[0]*dxx[0] - E[1]*dxx[1] ; + n[1] = n[1] - E[3]*dxx[0] - E[4]*dxx[1] ; + + // Step 11. nn -= nps.matmult(dx, E[:2,:2]) + nn[0] = nn[0] - E[0]*dx[0] - E[3]*dx[1] ; + nn[1] = nn[1] - E[1]*dx[0] - E[4]*dx[1] ; + + // Step 12. l *= 2*d/( nps.inner(n,n) + nps.inner(nn,nn) ) + val_withgrad_t<18> bb = (n [0]*n [0] + n [1]*n [1] + + nn[0]*nn[0] + nn[1]*nn[1]) * 0.5; + l = l/d * bb; + + // Step 13. dx = l*n + dx[0] = l * n[0]; + dx[1] = l * n[1]; + + // Step 14. dxx = l*nn + dxx[0] = l * nn[0]; + dxx[1] = l * nn[1]; + + // Step 15 + v0.v[0] = x0[0] - dx[0]; + v0.v[1] = x0[1] - dx[1]; + v0.v[2] = val_withgrad_t<18>(1.0); + + // Step 16 + v1.v[0] = x1[0] - dxx[0]; + v1.v[1] = x1[1] - dxx[1]; + v1.v[2] = val_withgrad_t<18>(1.0); + + // for debugging +#if 0 + { + double Ex1[3] = { E[0].x*v1[0].x + E[1].x*v1[1].x + E[2].x, + E[3].x*v1[0].x + E[4].x*v1[1].x + E[5].x, + E[6].x*v1[0].x + E[7].x*v1[1].x + E[8].x }; + double x0Ex1 = Ex1[0]*v0[0].x + Ex1[1]*v0[1].x + Ex1[2]; + fprintf(stderr, "conj after: %f\n", x0Ex1); + } +#endif + + // Construct v0, v1 in a common coord system + vec_withgrad_t<18,3> Rv1; + Rv1.v[0] = R01.v[0]*v1.v[0] + R01.v[1]*v1.v[1] + R01.v[2]*v1.v[2]; + Rv1.v[1] = R01.v[3]*v1.v[0] + R01.v[4]*v1.v[1] + R01.v[5]*v1.v[2]; + Rv1.v[2] = R01.v[6]*v1.v[0] + R01.v[7]*v1.v[1] + R01.v[8]*v1.v[2]; + + // My two 3D rays now intersect exactly, and I use compute the intersection + // with that assumption + vec_withgrad_t<18,3> m; + if(!triangulate_assume_intersect(m, v0, Rv1, t01)) + return (mrcal_point3_t){0}; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dRt01 != NULL) + m.extract_grad (_dm_dRt01->xyz, 6,12,0, + 12*sizeof(double), sizeof(double), + 3); + return _m; +} + +// Minimize L1 angle error. Described in "Closed-Form Optimal Two-View +// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV +// 2019. +extern "C" +mrcal_point3_t +mrcal_triangulate_leecivera_l1(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_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) +{ + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + val_withgrad_t<9> dot_v0v0 = v0.norm2(); + val_withgrad_t<9> dot_v1v1 = v1.norm2(); + val_withgrad_t<9> dot_v0t = v0.dot(t01); + val_withgrad_t<9> dot_v1t = v1.dot(t01); + + // I pick a bath based on which len(cross(normalize(m),t)) is larger: which + // of m0 and m1 is most perpendicular to t. I can use a simpler dot product + // here instead: the m that is most perpendicular to t will have the + // smallest dot product. + // + // len(cross(m0/len(m0), t)) < len(cross(m1/len(m1), t)) ~ + // len(cross(v0/len(v0), t)) < len(cross(v1/len(v1), t)) ~ + // abs(dot(v0/len(v0), t)) > abs(dot(v1/len(v1), t)) ~ + // (dot(v0/len(v0), t))^2 > (dot(v1/len(v1), t))^2 ~ + // (dot(v0, t))^2 norm2(v1) > (dot(v1, t))^2 norm2(v0) ~ + if(dot_v0t.x*dot_v0t.x * dot_v1v1.x > dot_v1t.x*dot_v1t.x * dot_v0v0.x ) + { + // Equation (12) + vec_withgrad_t<9,3> n1 = cross<9>(v1, t01); + v0 -= n1 * v0.dot(n1)/n1.norm2(); + } + else + { + // Equation (13) + vec_withgrad_t<9,3> n0 = cross<9>(v0, t01); + v1 -= n0 * v1.dot(n0)/n0.norm2(); + } + + // My two 3D rays now intersect exactly, and I use compute the intersection + // with that assumption + + vec_withgrad_t<9,3> m; + if(!triangulate_assume_intersect(m, v0, v1, t01)) + return (mrcal_point3_t){0}; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3,0, + 3*sizeof(double), sizeof(double), + 3); + + return _m; +} + +// Minimize L-infinity angle error. Described in "Closed-Form Optimal Two-View +// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV +// 2019. +extern "C" +mrcal_point3_t +mrcal_triangulate_leecivera_linf(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_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) +{ + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + v0 /= v0.mag(); + v1 /= v1.mag(); + + vec_withgrad_t<9,3> na = cross<9>(v0 + v1, t01); + vec_withgrad_t<9,3> nb = cross<9>(v0 - v1, t01); + + vec_withgrad_t<9,3>& n = + ( na.norm2().x > nb.norm2().x ) ? + na : nb; + + v0 -= n * v0.dot(n)/n.norm2(); + v1 -= n * v1.dot(n)/n.norm2(); + + // My two 3D rays now intersect exactly, and I use compute the intersection + // with that assumption + vec_withgrad_t<9,3> m; + if(!triangulate_assume_intersect(m, v0, v1, t01)) + return (mrcal_point3_t){0}; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3,0, + 3*sizeof(double), sizeof(double), + 3); + + 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) +{ + 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++) + { + 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; + } + + return + improvement0.x > 0.0 && + improvement1.x > 0.0 && + improvement01.x > 0.0; +} + +// 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); +} + +// The "Mid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier +// Civera. https://arxiv.org/abs/1907.11917 +extern "C" +mrcal_point3_t +mrcal_triangulate_leecivera_mid2(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_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) +{ + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + val_withgrad_t<9> p_norm2_recip = val_withgrad_t<9>(1.0) / cross_norm2<9>(v0, v1); + + val_withgrad_t<9> l0 = (cross_norm2<9>(v1, t01) * p_norm2_recip).sqrt(); + val_withgrad_t<9> l1 = (cross_norm2<9>(v0, t01) * p_norm2_recip).sqrt(); + + if(!chirality(l0, v0, l1, v1, t01)) + return (mrcal_point3_t){0}; + + vec_withgrad_t<9,3> m = (v0*l0 + t01+v1*l1) / 2.0; + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3,0, + 3*sizeof(double), sizeof(double), + 3); + + 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" +mrcal_point3_t +mrcal_triangulate_leecivera_wmid2(// outputs + // These all may be NULL + mrcal_point3_t* _dm_dv0, + mrcal_point3_t* _dm_dv1, + mrcal_point3_t* _dm_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) +{ + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<9,3> v0 (_v0 ->xyz, 0); + vec_withgrad_t<9,3> v1 (_v1 ->xyz, 3); + vec_withgrad_t<9,3> t01(_t01->xyz, 6); + + // Unlike Mid2 I need to normalize these here to make the math work. l0 and + // l1 now have units of m, and I weigh by 1/l0 and 1/l1 + v0 /= v0.mag(); + v1 /= v1.mag(); + + val_withgrad_t<9> p_mag_recip = val_withgrad_t<9>(1.0) / cross_mag<9>(v0, v1); + + val_withgrad_t<9> l0 = cross_mag<9>(v1, t01) * p_mag_recip; + val_withgrad_t<9> l1 = cross_mag<9>(v0, t01) * p_mag_recip; + + if(!chirality(l0, v0, l1, v1, t01)) + return (mrcal_point3_t){0}; + + vec_withgrad_t<9,3> m = (v0*l0*l1 + t01*l0 + v1*l0*l1) / (l0 + l1); + + mrcal_point3_t _m; + m.extract_value(_m.xyz); + + if(_dm_dv0 != NULL) + m.extract_grad (_dm_dv0->xyz, 0,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dv1 != NULL) + m.extract_grad (_dm_dv1->xyz, 3,3, 0, + 3*sizeof(double), sizeof(double), + 3); + if(_dm_dt01 != NULL) + m.extract_grad (_dm_dt01->xyz, 6,3,0, + 3*sizeof(double), sizeof(double), + 3); + + 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.; + + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: temporary hack to avoid dividing by 0" +#endif + 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(); +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #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" +#endif +} + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: maybe exposing the triangulated-error C function is OK? I'm already exposing the Python function" +#endif + + +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.; + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #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" +#endif + +#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 + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: set reasonable thresholds" +#endif +#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 +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: temporary testing logic" +#endif +#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); + +#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS +// #warning "triangulated-solve: temporary testing logic" +#endif +#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; +} diff --git a/wpical/src/main/native/thirdparty/mrcal_java/include/mrcal_wrapper.h b/wpical/src/main/native/thirdparty/mrcal_java/include/mrcal_wrapper.h new file mode 100644 index 00000000000..0a36d4702d5 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal_java/include/mrcal_wrapper.h @@ -0,0 +1,84 @@ +/* + * Copyright (C) Photon Vision. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ + +#pragma once + +extern "C" { +// Seems to be missing C++ guards +#include + +} // extern "C" + +// Seems like these people don't properly extern-c their headers either +extern "C" { +#include +#include +} // extern "C" + +#include +#include +#include +#include +#include + +struct mrcal_result { + bool success; + std::vector intrinsics; + double rms_error; + std::vector residuals; + mrcal_calobject_warp_t calobject_warp; + int Noutliers_board; + // TODO standard devs + + mrcal_result() = default; + mrcal_result(bool success_, std::vector intrinsics_, + double rms_error_, std::vector residuals_, + mrcal_calobject_warp_t calobject_warp_, int Noutliers_board_) + : success{success_}, intrinsics{std::move(intrinsics_)}, + rms_error{rms_error_}, residuals{std::move(residuals_)}, + calobject_warp{calobject_warp_}, Noutliers_board{Noutliers_board_} {} + mrcal_result(mrcal_result &&) = delete; + ~mrcal_result(); +}; + +mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, + cv::Size boardSize, cv::Size imagerSize, + double squareSize, double focal_len_guess); + +std::unique_ptr mrcal_main( + // List, depth is ordered array observation[N frames, object_height, + // object_width] = [x,y, weight] weight<0 means ignored) + std::span observations_board, + // RT transform from camera to object + std::span frames_rt_toref, + // Chessboard size, in corners (not squares) + cv::Size calobjectSize, double boardSpacing, + // res, pixels + cv::Size cameraRes, + // focal length, in pixels + double focal_len_guess); + +enum class CameraLensModel { + LENSMODEL_OPENCV5 = 0, + LENSMODEL_OPENCV8, + LENSMODEL_STEREOGRAPHIC, + LENSMODEL_SPLINED_STEREOGRAPHIC +}; + +bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat, + const cv::Mat *distCoeffs, CameraLensModel lensModel, + // Extra stuff for splined stereographic models + uint16_t order, uint16_t Nx, uint16_t Ny, + uint16_t fov_x_deg); diff --git a/wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp b/wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp new file mode 100644 index 00000000000..31c0da37a18 --- /dev/null +++ b/wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp @@ -0,0 +1,614 @@ +/* + * Copyright (C) Photon Vision. + * + * Permission to use, copy, modify, and/or distribute this software for any + * purpose with or without fee is hereby granted. + * + * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES + * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF + * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY + * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES + * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION + * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN + * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. + */ + +#include "mrcal_wrapper.h" + + +#include + +#include +#include +#include +#include +#include +#include +#include + +using namespace cv; + +class CholmodCtx { +public: + cholmod_common Common, *cc; + CholmodCtx() { + cc = &Common; + cholmod_l_start(cc); + } + + ~CholmodCtx() { cholmod_l_finish(cc); } +}; +static CholmodCtx cctx; + +#define BARF(...) std::fprintf(stderr, __VA_ARGS__) + +// forward declarations for functions borrowed from mrcal-pywrap +static mrcal_problem_selections_t construct_problem_selections( + int do_optimize_intrinsics_core, int do_optimize_intrinsics_distortions, + int do_optimize_extrinsics, int do_optimize_frames, + int do_optimize_calobject_warp, int do_apply_regularization, + int do_apply_outlier_rejection, int Ncameras_intrinsics, + int Ncameras_extrinsics, int Nframes, int Nobservations_board); + +bool lensmodel_one_validate_args(mrcal_lensmodel_t *mrcal_lensmodel, + std::vector intrinsics, + bool do_check_layout); + +mrcal_point3_t* observations_point = nullptr; +mrcal_pose_t* + extrinsics_rt_fromref = nullptr; // Always zero for single camera, it seems? +mrcal_point3_t* points = nullptr; // Seems to always to be None for single camera? + +static std::unique_ptr mrcal_calibrate( + // List, depth is ordered array observation[N frames, object_height, + // object_width] = [x,y, weight] weight<0 means ignored) + std::span observations_board, + // RT transform from camera to object + std::span frames_rt_toref, + // Chessboard size, in corners (not squares) + Size calobjectSize, double calibration_object_spacing, + // res, pixels + Size cameraRes, + // solver options + mrcal_problem_selections_t problem_selections, + // seed intrinsics/cameramodel to optimize for + mrcal_lensmodel_t mrcal_lensmodel, std::vector intrinsics) { + // Number of board observations we've got. List of boards. in python, it's + // (number of chessboard pictures) x (rows) x (cos) x (3) + // hard-coded to 8, since that's what I've got below + int Nobservations_board = frames_rt_toref.size(); + + // Looks like this is hard-coded to 0 in Python for initial single-camera + // solve? + int Nobservations_point = 0; + + int calibration_object_width_n = + calobjectSize.width; // Object width, in # of corners + int calibration_object_height_n = + calobjectSize.height; // Object height, in # of corners + + // TODO set sizes and populate + int imagersize[] = {cameraRes.width, cameraRes.height}; + + mrcal_calobject_warp_t calobject_warp = {0, 0}; + + // int Nobservations_point_triangulated = 0; // no clue what this is + + int Npoints = 0; // seems like this is also unused? whack + int Npoints_fixed = 0; // seems like this is also unused? whack + + // Number of cameras to solve for intrinsics + int Ncameras_intrinsics = 1; + + // Hard-coded to match out 8 frames from above (borrowed from python) + std::vector indices_frame_camintrinsics_camextrinsics; + // Frame index, camera number, (camera number)-1??? + for (int i = 0; i < Nobservations_board; i++) { + indices_frame_camintrinsics_camextrinsics.push_back( + {static_cast(i), 0, -1}); + } + + // Pool is the raw observation backing array + mrcal_point3_t *c_observations_board_pool = (observations_board.data()); + mrcal_point3_t *c_observations_point_pool = observations_point; + + // Copy from board/point pool above, using some code borrowed from + // mrcal-pywrap + std::vector observations_board_data(Nobservations_board); + auto c_observations_board = observations_board_data.data(); + // Try to make sure we don't accidentally make a zero-length array or + // something stupid + std::vector + observations_point_data(std::max(Nobservations_point, 1)); + mrcal_observation_point_t* + c_observations_point = observations_point_data.data(); + + for (int i_observation = 0; i_observation < Nobservations_board; + i_observation++) { + int32_t iframe = + indices_frame_camintrinsics_camextrinsics.at(i_observation).x; + int32_t icam_intrinsics = + indices_frame_camintrinsics_camextrinsics.at(i_observation).y; + int32_t icam_extrinsics = + indices_frame_camintrinsics_camextrinsics.at(i_observation).z; + + c_observations_board[i_observation].icam.intrinsics = icam_intrinsics; + c_observations_board[i_observation].icam.extrinsics = icam_extrinsics; + c_observations_board[i_observation].iframe = iframe; + } + for (int i_observation = 0; i_observation < Nobservations_point; + i_observation++) { + int32_t i_point = + indices_frame_camintrinsics_camextrinsics.at(i_observation).x; + int32_t icam_intrinsics = + indices_frame_camintrinsics_camextrinsics.at(i_observation).y; + int32_t icam_extrinsics = + indices_frame_camintrinsics_camextrinsics.at(i_observation).z; + + c_observations_point[i_observation].icam.intrinsics = icam_intrinsics; + c_observations_point[i_observation].icam.extrinsics = icam_extrinsics; + c_observations_point[i_observation].i_point = i_point; + } + + int Ncameras_extrinsics = 0; // Seems to always be zero for single camera + int Nframes = + frames_rt_toref.size(); // Number of pictures of the object we've got + mrcal_observation_point_triangulated_t *observations_point_triangulated = NULL; + // NULL; + + if (!lensmodel_one_validate_args(&mrcal_lensmodel, intrinsics, false)) { + auto ret = std::make_unique(); + ret->success = false; + return ret; + } + + int Nstate = mrcal_num_states( + Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, + Nobservations_board, problem_selections, &mrcal_lensmodel); + + int Nmeasurements = mrcal_num_measurements( + Nobservations_board, Nobservations_point, + observations_point_triangulated, + 0, // hard-coded to 0 + calibration_object_width_n, calibration_object_height_n, + Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, + problem_selections, &mrcal_lensmodel); + + // OK, now we should have everything ready! Just some final setup and then + // call optimize + + // Residuals + std::vector b_packed_final(Nstate); + auto c_b_packed_final = b_packed_final.data(); + + std::vector x_final(Nmeasurements); + auto c_x_final = x_final.data(); + + // Seeds + double *c_intrinsics = intrinsics.data(); + mrcal_pose_t *c_extrinsics = extrinsics_rt_fromref; + mrcal_pose_t *c_frames = frames_rt_toref.data(); + mrcal_point3_t *c_points = points; + mrcal_calobject_warp_t *c_calobject_warp = &calobject_warp; + + // in + int *c_imagersizes = imagersize; + auto point_min_range = -1.0, point_max_range = -1.0; + mrcal_problem_constants_t problem_constants = { + .point_min_range = point_min_range, .point_max_range = point_max_range}; + int verbose = 0; + + auto stats = mrcal_optimize( + NULL, -1, c_x_final, Nmeasurements * sizeof(double), c_intrinsics, + c_extrinsics, c_frames, c_points, c_calobject_warp, Ncameras_intrinsics, + Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed, + c_observations_board, c_observations_point, Nobservations_board, + Nobservations_point, + observations_point_triangulated, -1, + c_observations_board_pool, c_observations_point_pool, &mrcal_lensmodel, c_imagersizes, + problem_selections, &problem_constants, calibration_object_spacing, + calibration_object_width_n, calibration_object_height_n, verbose, false); + + std::vector residuals = {c_x_final, c_x_final + Nmeasurements}; + return std::make_unique( + true, intrinsics, stats.rms_reproj_error__pixels, residuals, + calobject_warp, stats.Noutliers_board); +} + +struct MrcalSolveOptions { + // If true, we solve for the intrinsics core. Applies only to those models + // that HAVE a core (fx,fy,cx,cy) + int do_optimize_intrinsics_core; + + // If true, solve for the non-core lens parameters + int do_optimize_intrinsics_distortions; + + // If true, solve for the geometry of the cameras + int do_optimize_extrinsics; + + // If true, solve for the poses of the calibration object + int do_optimize_frames; + + // If true, optimize the shape of the calibration object + int do_optimize_calobject_warp; + + // If true, apply the regularization terms in the solver + int do_apply_regularization; + + // Whether to try to find NEW outliers. The outliers given on + // input are respected regardless + int do_apply_outlier_rejection; +}; + +// lifted from mrcal-pywrap.c. Restrict a given selection to only valid options +// License for mrcal-pywrap.c: +// 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 +static mrcal_problem_selections_t +construct_problem_selections(MrcalSolveOptions s, int Ncameras_intrinsics, + int Ncameras_extrinsics, int Nframes, + int Nobservations_board) { + // By default we optimize everything we can + if (s.do_optimize_intrinsics_core < 0) + s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0; + if (s.do_optimize_intrinsics_distortions < 0) + s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0; + if (s.do_optimize_extrinsics < 0) + s.do_optimize_extrinsics = Ncameras_extrinsics > 0; + if (s.do_optimize_frames < 0) + s.do_optimize_frames = Nframes > 0; + if (s.do_optimize_calobject_warp < 0) + s.do_optimize_calobject_warp = Nobservations_board > 0; + return { + .do_optimize_intrinsics_core = + static_cast(s.do_optimize_intrinsics_core), + .do_optimize_intrinsics_distortions = + static_cast(s.do_optimize_intrinsics_distortions), + .do_optimize_extrinsics = static_cast(s.do_optimize_extrinsics), + .do_optimize_frames = static_cast(s.do_optimize_frames), + .do_optimize_calobject_warp = + static_cast(s.do_optimize_calobject_warp), + .do_apply_regularization = static_cast(s.do_apply_regularization), + .do_apply_outlier_rejection = + static_cast(s.do_apply_outlier_rejection), + // .do_apply_regularization_unity_cam01 = false + }; +} + +bool lensmodel_one_validate_args(mrcal_lensmodel_t *mrcal_lensmodel, + std::vector intrinsics, + bool do_check_layout) { + int NlensParams = mrcal_lensmodel_num_params(mrcal_lensmodel); + int NlensParams_have = intrinsics.size(); + if (NlensParams != NlensParams_have) { + BARF("intrinsics.shape[-1] MUST be %d. Instead got %d", NlensParams, + NlensParams_have); + return false; + } + + return true; +} + +mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool, + Size boardSize, Size imagerSize, double squareSize, + double focal_len_guess) { + using std::vector, std::runtime_error; + + if (!c_observations_board_pool) { + throw runtime_error("board was null"); + } + + double fx = focal_len_guess; + double fy = fx; + double cx = (imagerSize.width / 2.0) - 0.5; + double cy = (imagerSize.height / 2.0) - 0.5; + + vector objectPoints; + vector imagePoints; + + // Fill in object/image points + for (int i = 0; i < boardSize.height; i++) { + for (int j = 0; j < boardSize.width; j++) { + auto &corner = c_observations_board_pool[i * boardSize.width + j]; + // weight<0 means ignored -- filter these out + if (corner.z >= 0) { + imagePoints.emplace_back(corner.x, corner.y); + objectPoints.push_back(Point3f(j * squareSize, i * squareSize, 0)); + } + } + } + + { + // convert from stereographic to pinhole to match python + std::vector mrcal_imagepts(imagePoints.size()); + std::transform( + imagePoints.begin(), imagePoints.end(), mrcal_imagepts.begin(), + [](const auto &pt) { return mrcal_point2_t{.x = pt.x, .y = pt.y}; }); + + mrcal_lensmodel_t model{.type = MRCAL_LENSMODEL_STEREOGRAPHIC}; + std::vector out(imagePoints.size()); + const double intrinsics[] = {fx, fy, cx, cy}; + bool ret = mrcal_unproject(out.data(), mrcal_imagepts.data(), + mrcal_imagepts.size(), &model, intrinsics); + if (!ret) { + std::cerr << "couldn't unproject!" << std::endl; + } + model = {.type = MRCAL_LENSMODEL_PINHOLE}; + mrcal_project(mrcal_imagepts.data(), NULL, NULL, out.data(), out.size(), + &model, intrinsics); + + std::transform(mrcal_imagepts.begin(), mrcal_imagepts.end(), + imagePoints.begin(), + [](const auto &pt) { return Point2d{pt.x, pt.y}; }); + } + + // Initial guess at intrinsics + Mat cameraMatrix = (Mat_(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1); + Mat distCoeffs = Mat(4, 1, CV_64FC1, Scalar(0)); + + Mat_ rvec, tvec; + vector objectPoints3; + for (auto a : objectPoints) + objectPoints3.push_back(Point3f(a.x, a.y, 0)); + + // for (auto& o : objectPoints) std::cout << o << std::endl; + // for (auto& i : imagePoints) std::cout << i << std::endl; + // std::cout << "cam mat\n" << cameraMatrix << std::endl; + // std::cout << "distortion: " << distCoeffs << std::endl; + + solvePnP(objectPoints3, imagePoints, cameraMatrix, distCoeffs, rvec, tvec, + false, SOLVEPNP_ITERATIVE); + + return mrcal_pose_t{.r = {.x = rvec(0), .y = rvec(1), .z = rvec(2)}, + .t = {.x = tvec(0), .y = tvec(1), .z = tvec(2)}}; +} + +mrcal_result::~mrcal_result() { + // cholmod_l_free_sparse(&Jt, cctx.cc); + return; +} + +// Code taken from mrcal, license: +// 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 +std::unique_ptr mrcal_main( + // List, depth is ordered array observation[N frames, object_height, + // object_width] = [x,y, weight] weight<0 means ignored) + std::span observations_board, + // RT transform from camera to object + std::span frames_rt_toref, + // Chessboard size, in corners (not squares) + Size calobjectSize, double calibration_object_spacing, + // res, pixels + Size cameraRes, double focal_length_guess) { + + std::unique_ptr result; + + { + // stereographic initial guess for intrinsics + double cx = (cameraRes.width / 2.0) - 0.5; + double cy = (cameraRes.height / 2.0) - 0.5; + std::vector intrinsics = {focal_length_guess, focal_length_guess, + cx, cy}; + + std::cout << "Initial solve (geometry only)" << std::endl; + + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = false, + .do_optimize_intrinsics_distortions = false, + .do_optimize_extrinsics = false, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = false, + .do_apply_outlier_rejection = false}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC; + + // And run calibration. This should mutate frames_rt_toref in place + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, intrinsics); + } + + { + std::cout + << "Initial solve (geometry and LENSMODEL_STEREOGRAPHIC core only)" + << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = false, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = false, + .do_apply_outlier_rejection = false}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC; + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, result->intrinsics); + } + + { + // Now we've got a seed, expand intrinsics to our target model + // see + // https://github.com/dkogan/mrcal/blob/33c3c50d5b7f991aca3a8e71ca52c5fffd153ef2/mrcal-calibrate-cameras#L533 + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + // num distortion params + int nparams = mrcal_lensmodel_num_params(&mrcal_lensmodel); + std::vector intrinsics(nparams); + + // copy core in + std::copy(result->intrinsics.begin(), result->intrinsics.end(), + intrinsics.begin()); + + // and generate random distortion + std::random_device rd; + std::mt19937 gen(rd()); + std::uniform_real_distribution<> dis(-0.5, 0.5); + + int nDistortion = nparams - 4; + std::vector seedDistortions(nDistortion); + + for (int j = 0; j < seedDistortions.size(); j++) { + if (j < 5) + seedDistortions[j] = dis(gen) * 2.0 * 1e-6; + else + seedDistortions[j] = dis(gen) * 2.0 * 1e-9; + } + + // copy distortion into our big intrinsics array + std::copy(seedDistortions.begin(), seedDistortions.end(), + intrinsics.begin() + result->intrinsics.size()); + + std::cout + << "Optimizing everything except board warp from seeded intrinsics" + << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = true, + .do_optimize_frames = true, + .do_optimize_calobject_warp = false, + .do_apply_regularization = true, + .do_apply_outlier_rejection = true}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, intrinsics); + } + + { + std::cout << "Final, full solve" << std::endl; + mrcal_problem_selections_t options = construct_problem_selections( + {.do_optimize_intrinsics_core = true, + .do_optimize_intrinsics_distortions = true, + .do_optimize_extrinsics = true, + .do_optimize_frames = true, + .do_optimize_calobject_warp = true, + .do_apply_regularization = true, + .do_apply_outlier_rejection = true}, + 1, 0, frames_rt_toref.size(), frames_rt_toref.size()); + + mrcal_lensmodel_t mrcal_lensmodel; + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + + result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize, + calibration_object_spacing, cameraRes, options, + mrcal_lensmodel, result->intrinsics); + } + + return result; +} + +bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat, + const cv::Mat *distCoeffs, CameraLensModel lensModel, + // Extra stuff for splined stereographic models + uint16_t order, uint16_t Nx, uint16_t Ny, + uint16_t fov_x_deg) { + mrcal_lensmodel_t mrcal_lensmodel; + switch (lensModel) { + case CameraLensModel::LENSMODEL_OPENCV5: + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV5; + break; + case CameraLensModel::LENSMODEL_OPENCV8: + mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8; + break; + case CameraLensModel::LENSMODEL_STEREOGRAPHIC: + mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC; + break; + case CameraLensModel::LENSMODEL_SPLINED_STEREOGRAPHIC: + mrcal_lensmodel.type = MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC; + + /* Maximum degree of each 1D polynomial. This is almost certainly 2 */ + /* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */ + mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order = order; + /* The horizontal field of view. Not including fov_y. It's proportional with + * Ny and Nx */ + mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.fov_x_deg = + fov_x_deg; + /* We have a Nx by Ny grid of control points */ + mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx = Nx; + mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny = Ny; + break; + default: + std::cerr << "Unknown lensmodel\n"; + return false; + } + + if (!(dst->cols == 2 && dst->cols == 2)) { + std::cerr << "Bad input array size\n"; + return false; + } + if (!(dst->type() == CV_64FC2 && dst->type() == CV_64FC2)) { + std::cerr << "Bad input type -- need CV_64F\n"; + return false; + } + if (!(dst->isContinuous() && dst->isContinuous())) { + std::cerr << "Bad input array -- need continuous\n"; + return false; + } + + // extract intrinsics core from opencv camera matrix + double fx = cameraMat->at(0, 0); + double fy = cameraMat->at(1, 1); + double cx = cameraMat->at(0, 2); + double cy = cameraMat->at(1, 2); + + // Core, distortion coefficients concatenated + int NlensParams = mrcal_lensmodel_num_params(&mrcal_lensmodel); + std::vector intrinsics(NlensParams); + intrinsics[0] = fx; + intrinsics[1] = fy; + intrinsics[2] = cx; + intrinsics[3] = cy; + for (size_t i = 0; i < distCoeffs->cols; i++) { + intrinsics[i + 4] = distCoeffs->at(i); + } + + // input points in the distorted image pixel coordinates + mrcal_point2_t *in = reinterpret_cast(dst->data); + // vec3 observation vectors defined up-to-length + std::vector out(dst->rows); + + mrcal_unproject(out.data(), in, dst->rows, &mrcal_lensmodel, + intrinsics.data()); + + // The output is defined "up-to-length" + // Let's project through pinhole again + + // Output points in pinhole pixel coordinates + mrcal_point2_t *pinhole_pts = reinterpret_cast(dst->data); + + size_t bound = dst->rows; + for (size_t i = 0; i < bound; i++) { + // from mrcal-project-internal/pinhole model + mrcal_point3_t &p = out[i]; + + double z_recip = 1. / p.z; + double x = p.x * z_recip; + double y = p.y * z_recip; + + pinhole_pts[i].x = x * fx + cx; + pinhole_pts[i].y = y * fy + cy; + } + + return true; +} diff --git a/wpical/src/main/native/win/wpical.ico b/wpical/src/main/native/win/wpical.ico new file mode 100644 index 00000000000..0d82d10298c Binary files /dev/null and b/wpical/src/main/native/win/wpical.ico differ diff --git a/wpical/src/main/native/win/wpical.rc b/wpical/src/main/native/win/wpical.rc new file mode 100644 index 00000000000..e8b7112c3fc --- /dev/null +++ b/wpical/src/main/native/win/wpical.rc @@ -0,0 +1 @@ +IDI_ICON1 ICON "wpical.ico" diff --git a/wpical/src/test/native/cpp/main.cpp b/wpical/src/test/native/cpp/main.cpp new file mode 100644 index 00000000000..a2b90c59137 --- /dev/null +++ b/wpical/src/test/native/cpp/main.cpp @@ -0,0 +1,10 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include + +int main(int argc, char** argv) { + ::testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/wpical/src/test/native/cpp/test_calibrate.cpp b/wpical/src/test/native/cpp/test_calibrate.cpp new file mode 100644 index 00000000000..c185face6f8 --- /dev/null +++ b/wpical/src/test/native/cpp/test_calibrate.cpp @@ -0,0 +1,89 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include + +#include + +#include +#include + +const std::string projectRootPath = PROJECT_ROOT_PATH; +const std::string calSavePath = + projectRootPath.substr(0, + projectRootPath.find("/src/main/native/resources")) + + "/build"; +cameracalibration::CameraModel cameraModel = { + .intrinsic_matrix = Eigen::Matrix::Identity(), + .distortion_coefficients = Eigen::Matrix::Zero(), + .avg_reprojection_error = 0.0}; +#ifdef __linux__ +const std::string fileSuffix = ".avi"; +const std::string videoLocation = "/altfieldvideo"; +#else +const std::string fileSuffix = ".mp4"; +const std::string videoLocation = "/fieldvideo"; +#endif +TEST(Camera_CalibrationTest, OpenCV_Typical) { + int ret = cameracalibration::calibrate( + projectRootPath + "/testcalibration" + fileSuffix, cameraModel, 0.709f, + 0.551f, 12, 8, false); + cameracalibration::dumpJson(cameraModel, + calSavePath + "/cameracalibration.json"); + EXPECT_EQ(ret, 0); +} + +TEST(Camera_CalibrationTest, OpenCV_Atypical) { + int ret = cameracalibration::calibrate( + projectRootPath + videoLocation + "/long" + fileSuffix, cameraModel, + 0.709f, 0.551f, 12, 8, false); + EXPECT_EQ(ret, 1); +} + +TEST(Camera_CalibrationTest, MRcal_Typical) { + int ret = cameracalibration::calibrate( + projectRootPath + "/testcalibration" + fileSuffix, cameraModel, .709f, 12, + 8, 1080, 1920, false); + EXPECT_EQ(ret, 0); +} + +TEST(Camera_CalibrationTest, MRcal_Atypical) { + int ret = cameracalibration::calibrate( + projectRootPath + videoLocation + "/short" + fileSuffix, cameraModel, + 0.709f, 12, 8, 1080, 1920, false); + EXPECT_EQ(ret, 1); +} + +TEST(Field_CalibrationTest, Typical) { + int ret = fieldcalibration::calibrate( + projectRootPath + videoLocation, calSavePath, + calSavePath + "/cameracalibration.json", + projectRootPath + "/2024-crescendo.json", 3, false); + EXPECT_EQ(ret, 0); +} + +TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) { + int ret = fieldcalibration::calibrate( + projectRootPath + videoLocation, calSavePath, + projectRootPath + videoLocation + "/long" + fileSuffix, + projectRootPath + "/2024-crescendo.json", 3, false); + EXPECT_EQ(ret, 1); +} + +TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) { + int ret = fieldcalibration::calibrate( + projectRootPath + videoLocation, calSavePath, + calSavePath + "/cameracalibration.json", + calSavePath + "/cameracalibration.json", 3, false); + EXPECT_EQ(ret, 1); +} + +TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) { + int ret = fieldcalibration::calibrate( + projectRootPath + "", calSavePath, + calSavePath + "/cameracalibration.json", + projectRootPath + "/2024-crescendo.json", 3, false); + EXPECT_EQ(ret, 1); +} diff --git a/wpical/src/test/native/cpp/test_result_is_exact.cpp b/wpical/src/test/native/cpp/test_result_is_exact.cpp new file mode 100644 index 00000000000..8ff313fe59a --- /dev/null +++ b/wpical/src/test/native/cpp/test_result_is_exact.cpp @@ -0,0 +1,167 @@ +// Copyright (c) FIRST and other WPILib contributors. +// Open Source Software; you can modify and/or share it under the terms of +// the WPILib BSD license file in the root directory of this project. + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +using namespace cv; + +struct cmpByFilename { + bool operator()(const std::string& a, const std::string& b) const { + auto a2 = std::stoi(a.substr(3, a.length() - 7)); + auto b2 = std::stoi(b.substr(3, b.length() - 7)); + // std::cout << a2 << " _ " << b2 << std::endl; + return a2 < b2; + } +}; + +std::vector calibrate(const std::string& fname, cv::Size boardSize, + cv::Size imagerSize) { + std::ifstream file(fname); + if (!file.is_open()) { + std::cerr << "Unable to open file corners.vnl" << std::endl; + return {}; + } + + std::string line; + std::map> points; + while (std::getline(file, line)) { + if (line.starts_with("#")) { + continue; + } + + if (line.empty()) { + continue; + } + + try { + std::istringstream iss(line); + std::string filename, x_str, y_str, level_str; + + iss >> filename >> x_str >> y_str >> level_str; + using std::stod; + + // From calibration.py: + // if weight_column_kind == 'level': the 4th column is a decimation level + // of the + // detected corner. If we needed to cut down the image resolution to + // detect a corner, its coordinates are known less precisely, and we use + // that information to weight the errors appropriately later. We set the + // output weight = 1/2^level. If the 4th column is '-' or <0, the given + // point was not detected, and should be ignored: we set weight = -1 + + double level; + if (level_str == "-") { + level = -1; + } else { + level = stod(level_str); + } + double weight; + if (level < 0) { + weight = -1; + } else { + weight = std::pow(0.5, level); + } + points[filename].push_back( + mrcal_point3_t{stod(x_str), stod(y_str), weight}); + // std::printf("put %s\n", *name); + } catch (std::exception const& e) { + std::perror(e.what()); + } + } + + file.close(); + + auto start = std::chrono::steady_clock::now(); + + std::vector observations_board; + std::vector frames_rt_toref; + // Pre-allocate worst case + size_t total_points = boardSize.width * boardSize.height * points.size(); + observations_board.reserve(total_points); + frames_rt_toref.reserve(points.size()); + + std::chrono::steady_clock::time_point begin = + std::chrono::steady_clock::now(); + + for (const auto& [key, value] : points) { + if (value.size()) { + auto ret = getSeedPose(value.data(), boardSize, imagerSize, 0.03, 1200); + + // Append to the Big List of board corners/levels + observations_board.insert(observations_board.end(), value.begin(), + value.end()); + // And list of pose seeds + frames_rt_toref.push_back(ret); + } else { + std::printf("No points for %s\n", key.c_str()); + } + } + + std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now(); + std::cout << "Seed took: " + << std::chrono::duration_cast(end - + begin) + .count() + << "[ms]" << std::endl; + + auto cal_result = mrcal_main(observations_board, frames_rt_toref, boardSize, + 0.030, imagerSize, 1200); + + auto dt = std::chrono::steady_clock::now() - start; + int dt_ms = dt.count(); + + auto& stats = *cal_result; + + double max_error = + *std::max_element(stats.residuals.begin(), stats.residuals.end()); + + if (1) { + std::printf("\n===============================\n\n"); + std::printf("RMS Reprojection Error: %.2f pixels\n", stats.rms_error); + std::printf("Worst residual (by measurement): %.1f pixels\n", max_error); + std::printf("Noutliers: %i of %zu (%.1f percent of the data)\n", + stats.Noutliers_board, total_points, + 100.0 * stats.Noutliers_board / total_points); + std::printf("calobject_warp: [%f, %f]\n", stats.calobject_warp.x2, + stats.calobject_warp.y2); + std::printf("dt, seeding + solve: %f ms\n", dt_ms / 1e6); + std::printf("Intrinsics [%zu]: ", stats.intrinsics.size()); + for (auto i : stats.intrinsics) + std::printf("%f ", i); + std::printf("\n"); + } + + return stats.intrinsics; +} + +const std::string projectRootPath = PROJECT_ROOT_PATH; + +TEST(MrcalResultExactlyMatchesTest, lifecam_1280) { + auto calculated_intrinsics{calibrate( + projectRootPath + "/lifecam_1280p_10x10.vnl", {10, 10}, {1280, 720})}; + + // ## generated with mrgingham --jobs 4 --gridn 10 + // /home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/*.png + // # generated on 2024-12-01 11:51:55 with /usr/bin/mrcal-calibrate-cameras + // --corners-cache corners.vnl --lensmodel LENSMODEL_OPENCV8 --focal 1200 + // --object-spacing 0.0254 --object-width-n 10 '*.png' + std::vector mrcal_cli_groundtruth_intrinsics{ + 1130.892767, 1129.149453, 609.9417222, 349.6457279, + 0.1489878273, -1.348622726, 0.002839630852, 0.001135629909, + 2.560627057, -0.03170208336, 0.0695788644, -0.09547554864}; + + for (int i = 0; i < 12; i++) { + EXPECT_NEAR(mrcal_cli_groundtruth_intrinsics[i], calculated_intrinsics[i], + 1e-6); + } +}