diff --git a/.github/workflows/comment-command.yml b/.github/workflows/comment-command.yml index 840761b0d4d..9e7bd97a853 100644 --- a/.github/workflows/comment-command.yml +++ b/.github/workflows/comment-command.yml @@ -87,8 +87,8 @@ jobs: python-version: 3.9 - name: Install jinja run: python -m pip install jinja2 - - name: Install protobuf dependencies - run: sudo apt-get update && sudo apt-get install -y protobuf-compiler && 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 + - name: Install protobuf and perl dependencies + run: sudo apt-get update && 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 - name: Regenerate all run: ./.github/workflows/pregen_all.py --quickbuf_plugin=protoc-gen-quickbuf-1.3.3-linux-x86_64.exe - name: Commit diff --git a/.github/workflows/pregen_all.py b/.github/workflows/pregen_all.py index d7ec35af74a..7bca587194b 100755 --- a/.github/workflows/pregen_all.py +++ b/.github/workflows/pregen_all.py @@ -19,6 +19,7 @@ def main(argv): subprocess.run(["python", f"{REPO_ROOT}/hal/generate_usage_reporting.py"]) subprocess.run(["python", f"{REPO_ROOT}/ntcore/generate_topics.py"]) subprocess.run(["python", f"{REPO_ROOT}/wpimath/generate_numbers.py"]) + subprocess.run(["python", f"{REPO_ROOT}/wpimath/generate_mrcal.py"]) subprocess.run( [ "python", diff --git a/.github/workflows/pregenerate.yml b/.github/workflows/pregenerate.yml index 692bef1609d..293b19eb51b 100644 --- a/.github/workflows/pregenerate.yml +++ b/.github/workflows/pregenerate.yml @@ -24,8 +24,8 @@ jobs: python-version: 3.9 - name: Install jinja run: python -m pip install jinja2 - - name: Install protobuf dependencies - run: sudo apt-get update && sudo apt-get install -y protobuf-compiler && 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 + - name: Install protobuf and perl dependencies + run: sudo apt-get update && 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 - name: Regenerate all run: python ./.github/workflows/pregen_all.py --quickbuf_plugin protoc-gen-quickbuf-1.3.3-linux-x86_64.exe - name: Add untracked files to index so they count as changes diff --git a/upstream_utils/mrcal.py b/upstream_utils/mrcal.py new file mode 100644 index 00000000000..7a2bfebe8db --- /dev/null +++ b/upstream_utils/mrcal.py @@ -0,0 +1,54 @@ +#!/usr/bin/env python3 + +import os +import shutil +import subprocess + +from upstream_utils import ( + walk_cwd_and_copy_if, + Lib, +) + + +def copy_upstream_src(wpilib_root): + wpical = os.path.join(wpilib_root, "wpimath") + + # 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) + + walk_cwd_and_copy_if( + lambda dp, f: (f.endswith(".h") or f.endswith(".hh")) + and not f.endswith("mrcal-image.h") + and not f.endswith("stereo.h") + and not f.endswith("stereo-matching-libelas.h") + and not dp.startswith(os.path.join(".", "test")), + os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), + ) + walk_cwd_and_copy_if( + lambda dp, f: (f.endswith(".c") or f.endswith(".cc") or f.endswith(".pl")) + and not f.endswith("mrcal-pywrap.c") + and not f.endswith("image.c") + and not f.endswith("stereo.c") + and not f.endswith("stereo-matching-libelas.cc") + and not f.endswith("uncertainty.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"), + ) + + +def main(): + name = "mrcal" + url = "https://github.com/dkogan/mrcal" + tag = "71c89c4e9f268a0f4fb950325e7d551986a281ec" + + mrcal = Lib(name, url, tag, copy_upstream_src) + mrcal.main() + + +if __name__ == "__main__": + main() diff --git a/wpimath/generate_mrcal.py b/wpimath/generate_mrcal.py new file mode 100755 index 00000000000..9fb845fc5a6 --- /dev/null +++ b/wpimath/generate_mrcal.py @@ -0,0 +1,35 @@ +#!/usr/bin/env python3 + + +import os +import subprocess +import sys +import argparse +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/wpimath/src/main/native/thirdparty/mrcal/include/autodiff.hh b/wpimath/src/main/native/thirdparty/mrcal/include/autodiff.hh new file mode 100644 index 00000000000..b6f217354c9 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/include/autodiff.hh @@ -0,0 +1,582 @@ +// 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 + +/* + Automatic differentiation routines. Used in poseutils-uses-autodiff.cc. See + that file for usage examples + */ + +// Apparently I need this in MSVC to get constants +#define _USE_MATH_DEFINES +#include +#include +#include "strides.h" + +template struct vec_withgrad_t; + + +template +struct val_withgrad_t +{ + double x; + double j[NGRAD]; + + __attribute__ ((visibility ("hidden"))) + val_withgrad_t(double _x = 0.0) : x(_x) + { + for(int i=0; i operator+( const val_withgrad_t& b ) const + { + val_withgrad_t y = *this; + y.x += b.x; + for(int i=0; i operator+( double b ) const + { + val_withgrad_t y = *this; + y.x += b; + return y; + } + __attribute__ ((visibility ("hidden"))) + void operator+=( const val_withgrad_t& b ) + { + *this = (*this) + b; + } + __attribute__ ((visibility ("hidden"))) + val_withgrad_t operator-( const val_withgrad_t& b ) const + { + val_withgrad_t y = *this; + y.x -= b.x; + for(int i=0; i operator-( double b ) const + { + val_withgrad_t y = *this; + y.x -= b; + return y; + } + __attribute__ ((visibility ("hidden"))) + void operator-=( const val_withgrad_t& b ) + { + *this = (*this) - b; + } + __attribute__ ((visibility ("hidden"))) + val_withgrad_t operator-() const + { + return (*this) * (-1); + } + __attribute__ ((visibility ("hidden"))) + val_withgrad_t operator*( const val_withgrad_t& b ) const + { + val_withgrad_t y; + 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; + } + __attribute__ ((visibility ("hidden"))) + void operator*=( const double b ) + { + *this = (*this) * b; + } + __attribute__ ((visibility ("hidden"))) + val_withgrad_t operator/( const val_withgrad_t& b ) const + { + val_withgrad_t y; + y.x = x / b.x; + for(int i=0; i operator/( double b ) const + { + return (*this) * (1./b); + } + __attribute__ ((visibility ("hidden"))) + void operator/=( const val_withgrad_t& b ) + { + *this = (*this) / b; + } + __attribute__ ((visibility ("hidden"))) + void operator/=( const double b ) + { + *this = (*this) / b; + } + __attribute__ ((visibility ("hidden"))) + val_withgrad_t sqrt(void) const + { + val_withgrad_t y; + 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]; + + vec_withgrad_t() {} + + __attribute__ ((visibility ("hidden"))) + void init_vars(const double* x_in, int ivar0, int Nvars, int i_gradvec0 = -1, + int stride = sizeof(double)) + { + // 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; + } + } + + __attribute__ ((visibility ("hidden"))) + vec_withgrad_t(const double* x_in, int i_gradvec0 = -1, + int stride = sizeof(double)) + { + init_vars(x_in, 0, NVEC, i_gradvec0, stride); + } + + __attribute__ ((visibility ("hidden"))) + val_withgrad_t& operator[](int i) + { + return v[i]; + } + + __attribute__ ((visibility ("hidden"))) + const val_withgrad_t& operator[](int i) const + { + return v[i]; + } + + __attribute__ ((visibility ("hidden"))) + void operator+=( const vec_withgrad_t& x ) + { + (*this) = (*this) + x; + } + __attribute__ ((visibility ("hidden"))) + vec_withgrad_t operator+( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) + x; + } + __attribute__ ((visibility ("hidden"))) + 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; + } + __attribute__ ((visibility ("hidden"))) + vec_withgrad_t operator-( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) - x; + } + __attribute__ ((visibility ("hidden"))) + 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; + } + __attribute__ ((visibility ("hidden"))) + vec_withgrad_t operator*( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) * x; + } + __attribute__ ((visibility ("hidden"))) + 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; + } + __attribute__ ((visibility ("hidden"))) + vec_withgrad_t operator/( const vec_withgrad_t& x ) const + { + vec_withgrad_t p; + for(int i=0; i& x ) + { + (*this) = (*this) / x; + } + __attribute__ ((visibility ("hidden"))) + 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; + } + + __attribute__ ((visibility ("hidden"))) + 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; + } + + __attribute__ ((visibility ("hidden"))) + val_withgrad_t norm2(void) const + { + return dot(*this); + } + + __attribute__ ((visibility ("hidden"))) + val_withgrad_t mag(void) const + { + val_withgrad_t l2 = norm2(); + return l2.sqrt(); + } + + __attribute__ ((visibility ("hidden"))) + void extract_value(double* out, + int stride = sizeof(double), + int ivar0 = 0, int Nvars = NVEC) const + { + 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/wpimath/src/main/native/thirdparty/mrcal/include/basic-geometry.h b/wpimath/src/main/native/thirdparty/mrcal/include/basic-geometry.h new file mode 100644 index 00000000000..769aa9d5478 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/include/basic-geometry.h @@ -0,0 +1,50 @@ +// 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; diff --git a/wpimath/src/main/native/thirdparty/mrcal/include/cahvore.h b/wpimath/src/main/native/thirdparty/mrcal/include/cahvore.h new file mode 100644 index 00000000000..4b6d3ddaddd --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h b/wpimath/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h new file mode 100644 index 00000000000..2930a5d6aa4 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h @@ -0,0 +1,1987 @@ +#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. + + +// Upper triangle is stored, in the usual row-major order. +__attribute__((unused)) +static +int index_sym33(int i, int j) +{ + // In the top-right triangle I have i<=j, and + // index = N*i+j - sum(i, i=0..i) + // = N*i+j - (i+1)*i/2 + // = (N*2 - i - 1)*i/2 + j + const int N=3; + if(i<=j) return (N*2-i-1)*i/2 + j; + else return (N*2-j-1)*j/2 + i; +} +__attribute__((unused)) +static +int index_sym33_assume_upper(int i, int j) +{ + const int N=3; + return (N*2-i-1)*i/2 + j; +} + +// Upper triangle is stored, in the usual row-major order. +__attribute__((unused)) +static +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; +} +__attribute__((unused)) +static +int index_sym66_assume_upper(int i, int j) +{ + const int N=6; + return (N*2-i-1)*i/2 + j; +} +__attribute__((unused)) +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 +__attribute__((unused)) +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; + } + } +} +__attribute__((unused)) +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 +__attribute__((unused)) +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 +__attribute__((unused)) +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/wpimath/src/main/native/thirdparty/mrcal/include/mrcal-internal.h b/wpimath/src/main/native/thirdparty/mrcal/include/mrcal-internal.h new file mode 100644 index 00000000000..4ec6acadb5f --- /dev/null +++ b/wpimath/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 {} 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; + +// 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/wpimath/src/main/native/thirdparty/mrcal/include/mrcal-types.h b/wpimath/src/main/native/thirdparty/mrcal/include/mrcal-types.h new file mode 100644 index 00000000000..87d05d8503c --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/include/mrcal-types.h @@ -0,0 +1,384 @@ +// 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 {} 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; + +#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) \ + _(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 +#warning "triangulated-solve: there should be a pool of these, and I should be indexing that pool" +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; + +#warning "triangulated-solve: triangulated points into a pool. maybe allowing the intrinsics to move in the process" + +// An observation of a discrete point where the point itself is NOT a part of +// the optimization, but computed implicitly via triangulation. This structure +// is very similar to mrcal_observation_point_t, except instead of i_point +// identifying the point being observed we have +#warning "triangulated-solve: FINISH DOC" +typedef struct +{ + // which camera is making this observation + mrcal_camera_index_t icam; + +#warning "triangulated-solve: DOCUMENT. CAN THIS BIT FIELD BE PACKED NICELY?" + // Set if this is the last camera observing this point. Any set of N>=2 + // cameras can observe any point. All observations of a given point are + // stored consecutively, the last one being noted by this bit +#warning "triangulated-solve: do I really need this? I cannot look at the next observation to determine when this one is done?" + bool last_in_set : 1; + +#warning "triangulated-solve: this is temporary. Should be a weight in observations_point_pool like all the other observations" + bool outlier : 1; + + // Observed pixel coordinates. This works just like elements of + // observations_board_pool and observations_point_pool + mrcal_point3_t px; +} mrcal_observation_point_triangulated_t; + + +#warning "triangulated-solve: need a function to identify a vanilla calibration problem. It needs to not include any triangulated points. The noise propagation is different" + + +// 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; + +#warning "triangulated-solve: Need finer-grained regularization flags" +#warning "triangulated-solve: Regularization flags should reflect do_optimize stuff and Ncameras stuff" + // If true, apply the regularization terms in the solver + bool do_apply_regularization : 1; + + // Whether to try to find NEW outliers. The outliers given on + // input are respected regardless + bool do_apply_outlier_rejection : 1; + + // Pull the distance between the first two cameras to 1.0 + bool do_apply_regularization_unity_cam01: 1; + +} mrcal_problem_selections_t; + +// Constants used in a mrcal optimization. This is similar to +// 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) +#warning "triangulated-solve: implement stats.Noutliers_triangulated_point; add to c-api.org" +#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/wpimath/src/main/native/thirdparty/mrcal/include/mrcal.h b/wpimath/src/main/native/thirdparty/mrcal/include/mrcal.h new file mode 100644 index 00000000000..ebb751eca30 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/include/mrcal.h @@ -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 + +#pragma once + +#include +#include + +#include "mrcal-types.h" +#include "poseutils.h" +#include "stereo.h" +#include "triangulation.h" +#include "mrcal-image.h" + +//////////////////////////////////////////////////////////////////////////////// +//////////////////// Lens models +//////////////////////////////////////////////////////////////////////////////// + +// 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 +__attribute__((unused)) +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); + +#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 + +// Public ABI stuff, that's not for end-user consumption +#include "mrcal-internal.h" + diff --git a/wpimath/src/main/native/thirdparty/mrcal/include/poseutils.h b/wpimath/src/main/native/thirdparty/mrcal/include/poseutils.h new file mode 100644 index 00000000000..7911f7e278e --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/include/poseutils.h @@ -0,0 +1,553 @@ +// 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" + ); diff --git a/wpimath/src/main/native/thirdparty/mrcal/include/scales.h b/wpimath/src/main/native/thirdparty/mrcal/include/scales.h new file mode 100644 index 00000000000..14ed88ab382 --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/include/strides.h b/wpimath/src/main/native/thirdparty/mrcal/include/strides.h new file mode 100644 index 00000000000..2c8ebf27f29 --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/include/triangulation.h b/wpimath/src/main/native/thirdparty/mrcal/include/triangulation.h new file mode 100644 index 00000000000..cb685f95000 --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/include/util.h b/wpimath/src/main/native/thirdparty/mrcal/include/util.h new file mode 100644 index 00000000000..bd151891efb --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/src/cahvore.cpp b/wpimath/src/main/native/thirdparty/mrcal/src/cahvore.cpp new file mode 100644 index 00000000000..c4ba29b1022 --- /dev/null +++ b/wpimath/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" +__attribute__ ((visibility ("hidden"))) +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/wpimath/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl b/wpimath/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl new file mode 100755 index 00000000000..044098b9f23 --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/src/mrcal.c b/wpimath/src/main/native/thirdparty/mrcal/src/mrcal.c new file mode 100644 index 00000000000..7fb77075fbc --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/src/mrcal.c @@ -0,0 +1,6851 @@ +// 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 +#include +#include +#include +#include + +#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) + + + +// 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; +} + +#warning "triangulated-solve: need known-range points, at-infinity points" + +int mrcal_num_measurements_points(int Nobservations_point) +{ + // 2: x,y measurements + return Nobservations_point * 2; +} + +#warning "triangulated-solve: Add a test for mrcal_measurement_index_points_triangulated()" +int mrcal_measurement_index_points_triangulated(int i_point_triangulated, + int Nobservations_board, + int Nobservations_point, + + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int calibration_object_height_n) +{ + if(observations_point_triangulated == NULL || + Nobservations_point_triangulated <= 0) + return -1; + + return + mrcal_num_measurements_boards(Nobservations_board, + calibration_object_width_n, + calibration_object_height_n) + + mrcal_num_measurements_points(Nobservations_point) + + mrcal_num_measurements_points_triangulated_initial_Npoints(observations_point_triangulated, + Nobservations_point_triangulated, + i_point_triangulated); +} + +#warning "triangulated-solve: python-wrap this function" +int mrcal_num_measurements_points_triangulated_initial_Npoints(// May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + // Only consider the leading Npoints. If Npoints < 0: take ALL the points + int Npoints) +{ + if(observations_point_triangulated == NULL || + Nobservations_point_triangulated <= 0) + return 0; + + // Similar loop as in _mrcal_num_j_nonzero(). If the data layout changes, + // update this and that + + int Nmeas = 0; + int ipoint = 0; + int iobservation = 0; + + while( iobservation < Nobservations_point_triangulated && + (Npoints < 0 || ipoint < Npoints)) + { + int Nset = 1; + while(!observations_point_triangulated[iobservation].last_in_set) + { + Nmeas += Nset++; + iobservation++; + } + + ipoint++; + iobservation++; + } + + return Nmeas; +} + +int mrcal_num_measurements_points_triangulated(// May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated) +{ + return + mrcal_num_measurements_points_triangulated_initial_Npoints( observations_point_triangulated, + Nobservations_point_triangulated, + -1 ); +} + +#warning "triangulated-solve: python-wrap this function" +bool mrcal_decode_observation_indices_points_triangulated( + // output + int* iobservation0, + int* iobservation1, + int* iobservation_point0, + int* Nobservations_this_point, + int* Nmeasurements_this_point, + int* ipoint, + + // input + const int imeasurement, + + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated) +{ + if(observations_point_triangulated == NULL || + Nobservations_point_triangulated <= 0) + return false; + + // Similar loop as in + // mrcal_num_measurements_points_triangulated_initial_Npoints(). If the data + // layout changes, update this and that + + int Nmeas = 0; + int iobservation = 0; + *ipoint = 0; + + while( iobservation < Nobservations_point_triangulated ) + { + int Nset = 1; + while(!observations_point_triangulated[iobservation].last_in_set) + { + Nset++; + iobservation++; + } + + // This point has Nset observations. Each pair of observations produces + // a measurement, so: + const int Nmeas_thispoint = Nset*(Nset-1) / 2; + + // Done with this point. It is described by Nmeas_thispoint + // measurements, with the first one at Nmeas. The last observation is at + // iobservation + + if(imeasurement < Nmeas+Nmeas_thispoint) + { + // The measurement I care about observes THIS point. I find the + // specific observations. + // + // I simplify the notation: "m" is "imeasurement", "o" is + // "iobservation". + + const int No = Nset; + const int Nm = Nmeas_thispoint; + const int m = imeasurement - Nmeas; + + // Within this point o is in range(No) and m is in range(Nm). A pair + // (o0,o1) describes one measurement. Both o0 and o1 are in + // range(No) and o1>o0. A sample table of measurement indices m for + // No = 5: + // + // o1 + // 0 1 2 3 4 + // --------- + // 0| 0 1 2 3 + // o0 1| 4 5 6 + // 2| 7 8 + // 3| 9 + // 4| + // + // For a given o0, the maximum m = + // + // m_max = (No-1) + (No-2) + (No-3) ... - 1 + // + // for a total of o0+1 (No...) terms so + // + // m_max = ((No-1) + (No-o0-1))/2 * (o0+1) - 1 + // = (2*No - 2 - o0)/2 * (o0+1) - 1 + // + // m_min = m_max(o0-1) + 1 + // = (2*No - 2 - o0 + 1)/2 * o0 + // = (2*No - 1 - o0) * o0 / 2 + // = (-o0^2 + (2*No - 1)*o0 ) / 2 + // + // -> (-1/2) o0^2 + (2*No - 1)/2 o0 - m_min = 0 + // -> o0 = (2*No - 1)/2 +- sqrt( (2*No - 1)^2/4 - 2*m_min) + // = (2*No - 1)/2 - sqrt( (2*No - 1)^2/4 - 2*m_min) + // = (2*No - 1 - sqrt( (2*No - 1)^2 - 8*m_min))/2 + // + // o0(m) = floor(o0(m)) + // + // Now that I have o0 I compute + // + // o1 = m - m_min(o0) + o0 + 1 + // = m - (-o0^2 + (2*No - 1)*o0 ) / 2 + o0 + 1 + // = m + (o0^2 + (3- 2*No)*o0 + 2) / 2 + // = m + o0*(o0 + 3 - 2*No)/2 + 1 + const double x = 2.*(double)No - 1.; + *iobservation0 = (int)((x - sqrt( x*x - 8.*(double)m))/2.); + *iobservation1 = m + (*iobservation0)*((*iobservation0) + 3 - 2*No)/2 + 1; + + // Reference the observations from the first + *iobservation_point0 = iobservation-Nset+1; + *iobservation0 += *iobservation_point0; + *iobservation1 += *iobservation_point0; + + *Nobservations_this_point = No; + *Nmeasurements_this_point = Nm; + + return true; + } + + Nmeas += Nmeas_thispoint; + + iobservation++; + (*ipoint)++; + } + + return false; +} + + +int mrcal_measurement_index_regularization( +#warning "triangulated-solve: this argument order is weird. Put then triangulated stuff at the end?" + // May be NULL if we don't have any of these + const mrcal_observation_point_triangulated_t* observations_point_triangulated, + int Nobservations_point_triangulated, + + int calibration_object_width_n, + int 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) + { +#warning "triangulated-solve: I assume that camera0 is at the reference" + 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] = {}; + double dpdistorted_ddistortion[3*NdistortionParams]; + + for(int i=0; i<3; i++) + { + double dmu_ddist[5] = { s__dmu_dalphabeta__domega_dalphabeta * domega_dalpha, + s__dmu_dalphabeta__domega_dalphabeta * domega_dbeta, + 1.0, + tau, + tau * tau }; + + dpdistorted_ddistortion[i*NdistortionParams + 0] = p->xyz[i] * dmu_ddist[0]; + dpdistorted_ddistortion[i*NdistortionParams + 1] = p->xyz[i] * dmu_ddist[1]; + dpdistorted_ddistortion[i*NdistortionParams + 2] = p->xyz[i] * dmu_ddist[2]; + dpdistorted_ddistortion[i*NdistortionParams + 3] = p->xyz[i] * dmu_ddist[3]; + dpdistorted_ddistortion[i*NdistortionParams + 4] = p->xyz[i] * dmu_ddist[4]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= dmu_ddist[0] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= dmu_ddist[1] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 2] -= dmu_ddist[2] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 3] -= dmu_ddist[3] * omega*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 4] -= dmu_ddist[4] * omega*o[i]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= mu * domega_dalpha*o[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= mu * domega_dbeta *o[i]; + + dpdistorted_ddistortion[i*NdistortionParams + 0] -= mu * omega * do_dalpha[i]; + dpdistorted_ddistortion[i*NdistortionParams + 1] -= mu * omega * do_dbeta [i]; + + dpdistorted_dpcam[3*i + i] = mu+1.0; + for(int j=0; j<3; j++) + { + dpdistorted_dpcam[3*i + j] += (p->xyz[i] - omega*o[i]) * dmu_dxyz[j]; + dpdistorted_dpcam[3*i + j] -= mu*o[i]*o[j]; + } + + p_distorted.xyz[i] = p->xyz[i] + mu * (p->xyz[i] - omega*o[i]); + } + + // q = fxy pxy/pz + cxy + // dqx/dp = d( fx px/pz + cx ) = fx/pz^2 (pz [1 0 0] - px [0 0 1]) + // dqy/dp = d( fy py/pz + cy ) = fy/pz^2 (pz [0 1 0] - py [0 0 1]) + const double fx = intrinsics[0]; + const double fy = intrinsics[1]; + const double cx = intrinsics[2]; + const double cy = intrinsics[3]; + double pz_recip = 1. / p_distorted.z; + q->x = p_distorted.x*pz_recip * fx + cx; + q->y = p_distorted.y*pz_recip * fy + cy; + + 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 + _Static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure"); + _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure"); + mrcal_compose_rt( _joint_rt, + gg._d_rj_rc, gg._d_rj_rf, + gg._d_tj_rc, gg._d_tj_tf, + camera_rt ->r.xyz, + frame_rt_validr.r.xyz); + joint_rt = _joint_rt; + } + else + { + // We're looking at the reference frame, so this camera transform is + // fixed at the identity. We don't need to compose anything, nor + // propagate gradients for the camera extrinsics, since those don't + // exist in the parameter vector + + // Here the "joint" transform is just the "frame" transform + joint_rt = frame_rt_validr.r.xyz; + } + + // Not using OpenCV distortions, the distortion and projection are not + // coupled + double Rj[3*3]; + double d_Rj_rj[9*3]; + + mrcal_R_from_r(Rj, d_Rj_rj, joint_rt); + + mrcal_point2_t* p_dq_dfxy = NULL; + double* p_dq_dintrinsics_nocore = NULL; + bool has_core = modelHasCore_fxfycxcy(lensmodel); + bool has_dense_intrinsics_grad = (lensmodel->type != MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + bool has_sparse_intrinsics_grad = (lensmodel->type == MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC); + 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 = + (gradient_sparse_meta_t) + { + .run_side_length = config->order+1, + .ivar_stridey = 2*config->Nx, + .pool = &dq_dintrinsics_pool_double[ivar_pool] + }; + 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 p = + _propagate_extrinsics( _dp_drc,_dp_dtc,_dp_drf,_dp_dtf, + &dp_drc,&dp_dtc,&dp_drf,&dp_dtf, + &(mrcal_point3_t){}, + camera_at_identity ? NULL : &gg, + Rj, d_Rj_rj, &joint_rt[3]); + _project_point( q, + p_dq_dfxy, p_dq_dintrinsics_nocore, + gradient_sparse_meta ? gradient_sparse_meta->pool : NULL, + runlen, + dq_drcamera, dq_dtcamera, dq_drframe, dq_dtframe, NULL, + &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; itype) || + 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, + &(mrcal_point3_t){.x = (q[i].x-cx)/fx, + .y = (q[i].y-cy)/fy, + .z = 1.}, + 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 ); + + _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++) + { + 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 ) + { + _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] ); + } + + 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; + } + + int icam_map_to_extrinsics[Ncameras_intrinsics]; + int 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; +#warning "triangulated-solve: outliers should not be marked in this first loop. This should happen in the following loop. Putting it here breaks the logic" + + + // There are a lot of these, so I'm disabling this print for + // now, to avoid spamming the terminal + // + // MSG("New divergent-feature outliers found: measurement %d observation (%d,%d)", + // imeasurement_point_triangulated0 + imeasurement_point_triangulated, + // i0, i1); + + } + } + // just marked divergent triangulations as outliers + + + if(pt0->outlier || pt1->outlier) + (*Noutliers_triangulated_point)++; + else + { + var += + x_point_triangulated[imeasurement_point_triangulated] * + x_point_triangulated[imeasurement_point_triangulated]; + Ninliers_triangulated_point++; + } + LOOP_TRIANGULATED_POINT_FOOTER(); + } + } + 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)++; + +#warning "triangulated-solve: outliers not returned to the caller yet, so I simply print them out here" + MSG("New outliers found: measurement %d observation (%d,%d)", + imeasurement_point_triangulated0 + imeasurement_point_triangulated, + i0, i1); + } + } + + LOOP_TRIANGULATED_POINT_FOOTER(); + } + } + + return true; + +#undef LOOP_BOARD_OBSERVATION +#undef LOOP_BOARD_FEATURE +#undef LOOP_BOARD_FEATURE_HEADER +#undef LOOP_TRIANGULATED_POINT0 +#undef LOOP_TRIANGULATED_POINT1 +#undef LOOP_TRIANGULATED_POINT_HEADER +#undef LOOP_TRIANGULATED_POINT_FOOTER +} + + +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 + double intrinsics_all[ctx->Ncameras_intrinsics][ctx->Nintrinsics]; + mrcal_pose_t 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]; + double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore]; + + int i_var_intrinsics = + mrcal_state_index_intrinsics(icam_intrinsics, + ctx->Ncameras_intrinsics, ctx->Ncameras_extrinsics, + ctx->Nframes, + ctx->Npoints, ctx->Npoints_fixed, ctx->Nobservations_board, + ctx->problem_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) ); + } + 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 + 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]; + // 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); + + 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]; + double* dq_dfxy = NULL; + double* dq_dintrinsics_nocore = NULL; + gradient_sparse_meta_t gradient_sparse_meta = {}; + + int splined_intrinsics_grad_irun = 0; + + project(q_hypothesis, + + ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ? + dq_dintrinsics_pool_double : 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 ? + (mrcal_point3_t*)dq_drcamera : NULL, + ctx->problem_selections.do_optimize_extrinsics ? + (mrcal_point3_t*)dq_dtcamera : NULL, + ctx->problem_selections.do_optimize_frames ? + (mrcal_point3_t*)dq_drframe : NULL, + ctx->problem_selections.do_optimize_frames ? + (mrcal_point3_t*)dq_dtframe : NULL, + has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ? + (mrcal_calobject_warp_t*)dq_dcalobject_warp : NULL, + + // input + intrinsics_all[icam_intrinsics], + &camera_rt[icam_extrinsics], &frame_rt, + ctx->calobject_warp == NULL ? NULL : &calobject_warp_local, + icam_extrinsics < 0, + &ctx->lensmodel, &ctx->precomputed, + ctx->calibration_object_spacing, + ctx->calibration_object_width_n, + ctx->calibration_object_height_n); + + for(int i_pt=0; + i_pt < ctx->calibration_object_width_n*ctx->calibration_object_height_n; + i_pt++, i_feature++) + { + const mrcal_point3_t* qx_qy_w__observed = &ctx->observations_board_pool[i_feature]; + double weight = qx_qy_w__observed->z; + + if(weight >= 0.0) + { + // I have my two measurements (dx, dy). I propagate their + // gradient and store them + for( int i_xy=0; i_xy<2; i_xy++ ) + { + const double err = (q_hypothesis[i_pt].xy[i_xy] - qx_qy_w__observed->xyz[i_xy]) * weight; + + if(ctx->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][i_xy].xyz[0] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_pt][i_xy].xyz[1] * + weight * SCALE_ROTATION_CAMERA, + dq_drcamera[i_pt][i_xy].xyz[2] * + weight * SCALE_ROTATION_CAMERA); + STORE_JACOBIAN3( i_var_camera_rt + 3, + dq_dtcamera[i_pt][i_xy].xyz[0] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_pt][i_xy].xyz[1] * + weight * SCALE_TRANSLATION_CAMERA, + dq_dtcamera[i_pt][i_xy].xyz[2] * + weight * SCALE_TRANSLATION_CAMERA); + } + + if( ctx->problem_selections.do_optimize_frames ) + { + STORE_JACOBIAN3( i_var_frame_rt + 0, + dq_drframe[i_pt][i_xy].xyz[0] * + weight * SCALE_ROTATION_FRAME, + dq_drframe[i_pt][i_xy].xyz[1] * + weight * SCALE_ROTATION_FRAME, + dq_drframe[i_pt][i_xy].xyz[2] * + weight * SCALE_ROTATION_FRAME); + STORE_JACOBIAN3( i_var_frame_rt + 3, + dq_dtframe[i_pt][i_xy].xyz[0] * + weight * SCALE_TRANSLATION_FRAME, + dq_dtframe[i_pt][i_xy].xyz[1] * + weight * SCALE_TRANSLATION_FRAME, + dq_dtframe[i_pt][i_xy].xyz[2] * + weight * SCALE_TRANSLATION_FRAME); + } + + if( has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ) + { + STORE_JACOBIAN_N( i_var_calobject_warp, + dq_dcalobject_warp[i_pt][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" + double 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 : 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], + &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); + } + +#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" + + 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 + { +#warning "triangulated-solve: better unity_cam01 scale" + 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."); + goto done; + } + + 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."); + goto done; + } + } + 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!"); + goto done; + } + + + 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); + goto done; + } + + 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); + goto done; + } + + 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, + .observations_point_pool = observations_point_pool, + .Nobservations_board = Nobservations_board, + .observations_point = observations_point, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, + .verbose = verbose, + .lensmodel = *lensmodel, + .imagersizes = imagersizes, + .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; + +done: + 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}; +#warning "triangulated-solve: can I loosen this? Optimizing intrinsics and triangulated points together should work" + } + + if(!modelHasCore_fxfycxcy(lensmodel)) + problem_selections.do_optimize_intrinsics_core = false; + + 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, + .observations_point_pool = observations_point_pool, + .Nobservations_board = Nobservations_board, + .observations_point = observations_point, + .Nobservations_point = Nobservations_point, + .observations_point_triangulated = observations_point_triangulated, + .Nobservations_point_triangulated = Nobservations_point_triangulated, + .verbose = verbose, + .lensmodel = *lensmodel, + .imagersizes = imagersizes, + .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? + double packed_state[Nstate]; + pack_solver_state(packed_state, + 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, +#warning "triangulated-solve: not const for now. this will become const once the outlier bit moves to the point_triangulated pool" + (mrcal_observation_point_triangulated_t*)observations_point_triangulated, + Nobservations_point_triangulated, + calibration_object_width_n, + calibration_object_height_n, + solver_context->beforeStep->x, + extrinsics_fromref, + verbose) && + ({MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again", + stats.Noutliers_board, + Nmeasurements_board, + (double)(stats.Noutliers_board * 100) / (double)Nmeasurements_board); + true;})); +#warning "triangulated-solve: the above print should deal with triangulated points too" + + + // Done. I have the final state. I spit it back out + unpack_solver_state( intrinsics, // Ncameras_intrinsics of these + extrinsics_fromref, // Ncameras_extrinsics of these + frames_toref, // Nframes of these + points, // Npoints of these + calobject_warp, + packed_state, + lensmodel, + problem_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)); + + done: + 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)); + goto done; + } + + int Nparams = mrcal_lensmodel_num_params(&cameramodel->lensmodel); + if(Nparams<0) + { + MSG("Couldn't get valid Nparams from lensmodel string '%s'", + lensmodel_string); + goto done; + } + + 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; + + done: + if(fp != NULL) + fclose(fp); + return result; +} + + +#warning "triangulated-solve: fixed points should live in a separate array, instead of at the end of the 'points' array" diff --git a/wpimath/src/main/native/thirdparty/mrcal/src/opencv.c b/wpimath/src/main/native/thirdparty/mrcal/src/opencv.c new file mode 100644 index 00000000000..3e751f611b1 --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c b/wpimath/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c new file mode 100644 index 00000000000..a0df80b52a7 --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp b/wpimath/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp new file mode 100644 index 00000000000..deb4670887e --- /dev/null +++ b/wpimath/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/wpimath/src/main/native/thirdparty/mrcal/src/poseutils.c b/wpimath/src/main/native/thirdparty/mrcal/src/poseutils.c new file mode 100644 index 00000000000..856397b18f7 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/src/poseutils.c @@ -0,0 +1,1080 @@ +// 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]; + } + } +} diff --git a/wpimath/src/main/native/thirdparty/mrcal/src/triangulation.cpp b/wpimath/src/main/native/thirdparty/mrcal/src/triangulation.cpp new file mode 100644 index 00000000000..a6807455b94 --- /dev/null +++ b/wpimath/src/main/native/thirdparty/mrcal/src/triangulation.cpp @@ -0,0 +1,1018 @@ +// 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.; + + +#warning "triangulated-solve: temporary hack to avoid dividing by 0" + if(th_sq.x < 1e-21) + { + return val_withgrad_t<6>(); + } + + + if(th_sq.x < 0) + // To handle roundoff errors + th_sq.x = 0; + + return th_sq.sqrt(); +#warning "triangulated-solve: look at numerical issues that will results in sqrt(<0)" +#warning "triangulated-solve: look at behavior near 0 where dsqrt/dx -> inf" +} + +#warning "triangulated-solve: maybe exposing the triangulated-error C function is OK? I'm already exposing the Python function" + +__attribute__((unused)) +static +double relu(double x, double knee) +{ + /* Smooth function ~ x>0 ? x : 0 + + Three modes + - x < 0: 0 + - 0 < x < knee: k*x^2 + - knee < x: x + eps + + At the transitions I want the function and the first derivative + to be continuous. At the knee I want d/dx = 1. So 2*k*knee = 1 -> + k = 1/(2*knee). k * knee^2 = knee + eps -> eps = knee * (k*knee - + 1) = -knee/2 + */ + if(x <= 0) return 0.0; + if(knee <= x) return x - knee/2.0; + + double k = 1. / (2*knee); + return k * x*x; +} + +static +val_withgrad_t<6> sigmoid(val_withgrad_t<6> x, double knee) +{ + /* Smooth function maps to 0..1 + + Modes + - x < 0: 0 + - 0 < x < knee: smooth interpolation + - knee < x: 1 + */ + if(x.x <= 0 ) return 0.0; + if(knee <= x.x) return 1.0; + + // transition at (x - knee/2.) < 0 + // f(x) = a x^2 + b x + c + // f(-knee/2.) = 0 + // f(0) = 1/2 + // f'(-knee/2.) = 0 + if(x.x < knee/2.0) + { + double b = 2./knee; + double a = b/knee; + double c = 1./2.; + return c + (x.x - knee/2.)*(b + (x.x - knee/2.)*a); + } + + // transition at (x - knee/2.) > 0 + // f(x) = a x^2 + b x + c + // f(knee/2.) = 1 + // f'(knee/2.) = 0 + // f(0) = 1/2 + { + double b = 2./knee; + double a = -b/knee; + double c = 1./2.; + return c + (x.x - knee/2.)*(b + (x.x - knee/2.)*a); + } +} + +// Internal function used in the optimization. This uses +// mrcal_triangulate_leecivera_mid2(), but contains logic in the divergent-ray +// case more appropriate for the optimization loop +extern "C" +double +_mrcal_triangulated_error(// outputs + mrcal_point3_t* _derr_dv1, + mrcal_point3_t* _derr_dt01, + + // inputs + + // not-necessarily normalized vectors in the camera-0 + // coord system + const mrcal_point3_t* _v0, + const mrcal_point3_t* _v1, + const mrcal_point3_t* _t01) +{ + ////////////////////////// Copy of mrcal_triangulate_leecivera_mid2(). I + ////////////////////////// extend it + + // Implementation here is a bit different: I don't propagate the gradient in + // respect to v0 + + // The paper has m0, m1 as the cam1-frame observation vectors. I do + // everything in cam0-frame + vec_withgrad_t<6,3> v0 (_v0 ->xyz, -1); // No gradient. Hopefully the + // compiler will collapse this + // aggressively + vec_withgrad_t<6,3> v1 (_v1 ->xyz, 0); + vec_withgrad_t<6,3> t01(_t01->xyz, 3); + + val_withgrad_t<6> p_norm2_recip = val_withgrad_t<6>(1.0) / cross_norm2<6>(v0, v1); + + val_withgrad_t<6> l0 = (cross_norm2<6>(v1, t01) * p_norm2_recip).sqrt(); + val_withgrad_t<6> l1 = (cross_norm2<6>(v0, t01) * p_norm2_recip).sqrt(); + + vec_withgrad_t<6,3> m = (v0*l0 + t01+v1*l1) / 2.0; + + // I compute the angle between the triangulated point and one of the + // observation rays, and I double this to measure from ray to ray + + // This is a fit error, which should be small. A small-angle cos() + // approximation is valid, unless the models don't fit at all. In which + // case a cos() error is the least of our issues + val_withgrad_t<6> err = angle_error__assume_small( v0, m ) * 2.; + +#warning "triangulated-solve: what happens when the rays are exactly parallel? Make sure the numerics remain happy. They don't: I divide by cross(v0,v1) ~ 0" + +#if 0 + // original method + if(!chirality(l0, v0, l1, v1, t01)) + { + // The rays diverge. This is aphysical, but an incorrect (i.e. + // not-yet-converged) geometry can cause this. Even if the optimization + // has converged, this can still happen if pixel noise or an incorrect + // feature association bumped converging rays to diverge. + // + // An obvious thing do wo would be to return the distance to the + // vanishing point. This creates a yaw bias however: barely convergent + // rays have zero effect on yaw, but barely divergent rays have a strong + // effect on yaw + // + // Goals: + // + // - There should be no qualitative change in the cost function as rays + // cross over from convergent to divergent. Low-error, parallel-ish + // rays look out to infinity, which means that these define yaw very + // poorly, and would affect the pitch, roll only. Yaw is what controls + // divergence, so if random noise makes rays diverge, we should use + // the error as before, to set our pitch, roll + // + // - Very divergent rays are bogus, and I do apply a penalty factor + // based on the divergence. This penalty factor begins to kick in only + // past a certain point, so there's no effect at the transition. This + // transition point should be connected to the outlier rejection + // threshold. + val_withgrad_t<6> err_to_vanishing_point = angle_error__assume_small(v0, v1); + + // I have three modes: + // + // - err_to_vanishing_point < THRESHOLD_DIVERGENT_LOWER + // I attribute the error to random noise, and simply report the + // reprojection error as before, ignoring the divergence. This will + // barely affect the yaw, but will pull the pitch and roll in the + // desired directions + // + // - err_to_vanishing_point > THRESHOLD_DIVERGENT_UPPER + // I add a term to pull the observation towards the vanishing point. + // This affects the yaw primarily, and does not touch the pitch and + // roll very much, since I don't have reliable information about them + // here + // + // - err_to_vanishing_point between THRESHOLD_DIVERGENT_LOWER and _UPPER + // I linearly the scale on this divergence term from 0 to 1 + +#warning "triangulated-solve: set reasonable thresholds" +#define THRESHOLD_DIVERGENT_LOWER 3.0e-4 +#define THRESHOLD_DIVERGENT_UPPER 6.0e-4 + + if(err_to_vanishing_point.x <= THRESHOLD_DIVERGENT_LOWER) + { + // We're barely divergent. Just do the normal thing + } + else if(err_to_vanishing_point.x >= THRESHOLD_DIVERGENT_UPPER) + { + // we're VERY divergent. Add another cost term: + // the distance to the vanishing point +#warning "triangulated-solve: temporary testing logic" +#if defined DIVERGENT_COST_ONLY && DIVERGENT_COST_ONLY + err = err_to_vanishing_point; +#else + err += err_to_vanishing_point; +#endif + err.x += 200.0; + } + else + { + // linearly interpolate. As + // err_to_vanishing_point lower->upper we + // produce k 0->1 + val_withgrad_t<6> k = + (err_to_vanishing_point - THRESHOLD_DIVERGENT_LOWER) / + (THRESHOLD_DIVERGENT_UPPER - THRESHOLD_DIVERGENT_LOWER); + +#warning "triangulated-solve: temporary testing logic" +#if defined DIVERGENT_COST_ONLY && DIVERGENT_COST_ONLY + err = k*err_to_vanishing_point + (val_withgrad_t<6>(1.0)-k)*err; +#else + err += k*err_to_vanishing_point; +#endif + err.x += 100.0; + } + } + +#else + + // new method + val_withgrad_t<6> improvement0; + val_withgrad_t<6> improvement1; + val_withgrad_t<6> improvement01; + if(!chirality(improvement0, improvement1, improvement01, + l0, v0, l1, v1, t01)) + { + val_withgrad_t<6> err_to_vanishing_point = angle_error__assume_small(v0, v1); + + err += + err_to_vanishing_point * (sigmoid(-improvement0, 3.0) + + sigmoid(-improvement1, 3.0) + + sigmoid(-improvement01, 3.0)); + } + +#endif + + if(_derr_dv1 != NULL) + memcpy(_derr_dv1->xyz, + &err.j[0], + 3*sizeof(double)); + if(_derr_dt01 != NULL) + memcpy(_derr_dt01->xyz, + &err.j[3], + 3*sizeof(double)); + + return err.x; +}