Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpical] Add WPIcal: Field Calibration Tool (#6915)

Browse Source 
Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com>
Co-authored-by: Jade <spacey-sooty@proton.me>
Co-authored-by: Matthew Morley <matthew.morley.ca@gmail.com>
This commit is contained in:
Elliot Scher
2024-12-28 23:24:32 -05:00
committed by GitHub
parent b74f84f876
commit 85507a6c65
86 changed files with 30195 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

8
.github/actions/pregen/action.yml vendored
View File

@@ -11,10 +11,10 @@ runs:
- name: Install jinja and protobuf
run: python -m pip install jinja2 protobuf grpcio-tools
shell: bash
- name: Install protobuf dependencies
- name: Install protobuf and perl dependencies
run: |
sudo apt-get update
sudo apt-get install -y protobuf-compiler
sudo apt-get install -y protobuf-compiler liblist-moreutils-perl
wget https://github.com/HebiRobotics/QuickBuffers/releases/download/1.3.3/protoc-gen-quickbuf-1.3.3-linux-x86_64.exe
chmod +x protoc-gen-quickbuf-1.3.3-linux-x86_64.exe
shell: bash
@@ -45,6 +45,10 @@ runs:
./wpilibj/generate_pwm_motor_controllers.py
shell: bash
- name: Regenerate mrcal minimath
run: ./wpical/generate_mrcal.py
shell: bash
- name: Regenerate wpimath
run: |
./wpimath/generate_nanopb.py

9
CMakeLists.txt
View File

@@ -76,6 +76,7 @@ cmake_dependent_option(
)
option(WITH_CSCORE "Build cscore (needs OpenCV)" ON)
option(WITH_NTCORE "Build ntcore" ON)
option(WITH_WPICAL "Build wpical" OFF)
option(WITH_WPIMATH "Build wpimath" ON)
cmake_dependent_option(
WITH_WPIUNITS
@@ -142,6 +143,11 @@ if(WITH_DOCS)
include(AddDoxygenDocs)
add_doxygen_docs()
endif()
if(WITH_WPICAL)
find_package(Ceres CONFIG REQUIRED)
endif()
find_package(LIBSSH CONFIG 0.7.1)
set(CMAKE_FIND_PACKAGE_PREFER_CONFIG ON)
@@ -314,6 +320,9 @@ if(WITH_GUI)
add_subdirectory(glass)
add_subdirectory(outlineviewer)
add_subdirectory(sysid)
if(WITH_WPICAL)
add_subdirectory(wpical)
endif()
if(LIBSSH_FOUND)
add_subdirectory(roborioteamnumbersetter)
add_subdirectory(datalogtool)

2
README.md
View File

@@ -59,6 +59,8 @@ Using Gradle makes building WPILib very straightforward. It only has a few depen
On macOS ARM, run `softwareupdate --install-rosetta`. This is necessary to be able to use the macOS x86 roboRIO toolchain on ARM.
On linux, run `sudo apt install gfortran`. This is necessary to be able to build WPIcal on linux platforms.
## Setup
Clone the WPILib repository and follow the instructions above for installing any required tooling. The build process uses versioning information from git. Downloading the source is not sufficient to run the build.

1
settings.gradle
View File

@@ -62,6 +62,7 @@ include 'epilogue-processor'
include 'epilogue-runtime'
include 'thirdparty:googletest'
include 'thirdparty:imgui_suite'
include 'wpical'
buildCache {
def cred = {

13
shared/ceres.gradle Normal file
View File

@@ -0,0 +1,13 @@
nativeUtils {
nativeDependencyContainer {
ceres(getNativeDependencyTypeClass('WPIStaticMavenDependency')) {
groupId = "edu.wpi.first.thirdparty.frc2024.ceres"
artifactId = "ceres-cpp"
headerClassifier = "headers"
sourceClassifier = "sources"
ext = "zip"
version = '2.2-3'
targetPlatforms.addAll(nativeUtils.wpi.platforms.desktopPlatforms)
}
}
}

58
upstream_utils/libdogleg.py Executable file
View File

@@ -0,0 +1,58 @@
#!/usr/bin/env python3
import os
import shutil
from upstream_utils import Lib, walk_cwd_and_copy_if
def copy_upstream_src(wpilib_root):
wpical = os.path.join(wpilib_root, "wpical")
# Delete old install
for d in [
"src/main/native/thirdparty/libdogleg/src",
"src/main/native/thirdparty/libdogleg/include",
]:
shutil.rmtree(os.path.join(wpical, d), ignore_errors=True)
files = walk_cwd_and_copy_if(
lambda dp, f: f.endswith("dogleg.h"),
os.path.join(wpical, "src/main/native/thirdparty/libdogleg/include"),
)
for f in files:
with open(f) as file:
content = file.read()
content = content.replace(
"#include <cholmod.h>", "#include <suitesparse/cholmod.h>"
)
with open(f, "w") as file:
file.write(content)
files = walk_cwd_and_copy_if(
lambda dp, f: f.endswith("dogleg.cpp"),
os.path.join(wpical, "src/main/native/thirdparty/libdogleg/src"),
)
for f in files:
with open(f) as file:
content = file.read()
content = content.replace("#warning", "// #warning")
content = content.replace("__attribute__((unused))", "")
content = content.replace(
"#include <cholmod_function.h>", "#include <suitesparse/cholmod_function.h>"
)
with open(f, "w") as file:
file.write(content)
def main():
name = "libdogleg"
url = "https://github.com/dkogan/libdogleg"
tag = "c971ea43088d286a3683c1039b9a85f761f7df15"
libdogleg = Lib(name, url, tag, copy_upstream_src)
libdogleg.main()
if __name__ == "__main__":
main()

38
upstream_utils/libdogleg_patches/0001-Convert-to-C.patch Normal file
View File

@@ -0,0 +1,38 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Fri, 29 Nov 2024 19:52:22 -0500
Subject: [PATCH 1/3] Convert to C++
---
dogleg.c => dogleg.cpp | 0
dogleg.h | 8 +++++++-
2 files changed, 7 insertions(+), 1 deletion(-)
rename dogleg.c => dogleg.cpp (100%)
diff --git a/dogleg.c b/dogleg.cpp
similarity index 100%
rename from dogleg.c
rename to dogleg.cpp
diff --git a/dogleg.h b/dogleg.h
index 74337263a89d3448f55e125db6702ebfa7588b97..4a23b669a30642d290549bb94a1c787f079d0647 100644
--- a/dogleg.h
+++ b/dogleg.h
@@ -126,7 +126,9 @@ typedef struct
} dogleg_solverContext_t;
-
+#ifdef __cplusplus
+extern "C" {
+#endif
// Fills in the given structure with the default parameter set
void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters);
@@ -293,3 +295,7 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu
int NoutlierFeatures,
dogleg_operatingPoint_t* point,
dogleg_solverContext_t* ctx);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif

79
upstream_utils/libdogleg_patches/0002-Replace-VLAs-with-vectors.patch Normal file
View File

@@ -0,0 +1,79 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Fri, 29 Nov 2024 19:54:54 -0500
Subject: [PATCH 2/3] Replace VLAs with vectors
---
dogleg.cpp | 17 +++++++++--------
1 file changed, 9 insertions(+), 8 deletions(-)
diff --git a/dogleg.cpp b/dogleg.cpp
index 7e4259c303e21a9f4c63ba16f1bf5df131935057..9ed95f64b232f41a51fe23d72885ecadd86dc065 100644
--- a/dogleg.cpp
+++ b/dogleg.cpp
@@ -6,6 +6,7 @@
// Apparently I need this in MSVC to get constants
#define _USE_MATH_DEFINES
+#include <vector>
#include <stdio.h>
#include <stdarg.h>
#include <math.h>
@@ -1907,7 +1908,7 @@ static bool getOutliernessFactors_dense( // output
// where A = Jo inv(JtJ) Jot
//
// A is symmetric. I store the upper triangle
- double A[featureSize*(featureSize+1)/2];
+ std::vector<double> A(featureSize*(featureSize+1)/2);
int iA=0;
for(int i=0; i<featureSize; i++)
for(int j=i; j<featureSize; j++, iA++)
@@ -1921,7 +1922,7 @@ static bool getOutliernessFactors_dense( // output
}
accum_outlierness_factor(&factors[i_feature],
&point->x[i_measurement],
- A, featureSize, *scale);
+ A.data(), featureSize, *scale);
}
result = true;
@@ -2008,7 +2009,7 @@ static bool getOutliernessFactors_sparse( // output
// where A = Jo inv(JtJ) Jot
//
// A is symmetric. I store the upper triangle
- double A[featureSize*(featureSize+1)/2];
+ std::vector<double> A(featureSize*(featureSize+1)/2);
int iA=0;
for(int i=0; i<featureSize; i++)
for(int j=i; j<featureSize; j++, iA++)
@@ -2027,7 +2028,7 @@ static bool getOutliernessFactors_sparse( // output
}
accum_outlierness_factor(&factors[i_feature],
&point->x[i_measurement],
- A, featureSize, *scale);
+ A.data(), featureSize, *scale);
}
result = true;
@@ -2212,8 +2213,8 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu
// This is Jt because cholmod thinks in terms of col-first instead of
// row-first
- int Jt_p[featureSize+1];
- int Jt_i[NstateActive*featureSize];
+ std::vector<int> Jt_p(featureSize+1);
+ std::vector<int> Jt_i(NstateActive*featureSize);
for(int i=0; i<=featureSize; i++)
{
Jt_p[i] = i*NstateActive;
@@ -2224,8 +2225,8 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu
cholmod_sparse Jt_query_sparse = {.nrow = ctx->Nstate,
.ncol = featureSize,
.nzmax = NstateActive*featureSize,
- .p = (void*)Jt_p,
- .i = (void*)Jt_i,
+ .p = (void*)Jt_p.data(),
+ .i = (void*)Jt_i.data(),
.x = (double*)JqueryFeature,
.sorted = 1,
.packed = 1,

324
upstream_utils/libdogleg_patches/0003-Fix-C-build-errors.patch Normal file
View File

@@ -0,0 +1,324 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Sun, 1 Dec 2024 00:28:14 -0500
Subject: [PATCH 3/3] Fix C++ build errors
---
dogleg.cpp | 110 +++++++++++++++++++++++++++++++----------------------
1 file changed, 65 insertions(+), 45 deletions(-)
diff --git a/dogleg.cpp b/dogleg.cpp
index 9ed95f64b232f41a51fe23d72885ecadd86dc065..c93f696a9427f0644b047308d2d99302cd246462 100644
--- a/dogleg.cpp
+++ b/dogleg.cpp
@@ -327,9 +327,9 @@ void _dogleg_testGradient(unsigned int var, const double* p0,
{
int is_sparse = NJnnz > 0;
- double* x0 = malloc(Nmeas * sizeof(double));
- double* x = malloc(Nmeas * sizeof(double));
- double* p = malloc(Nstate * sizeof(double));
+ double* x0 = static_cast<double*>(malloc(Nmeas * sizeof(double)));
+ double* x = static_cast<double*>(malloc(Nmeas * sizeof(double)));
+ double* p = static_cast<double*>(malloc(Nstate * sizeof(double)));
ASSERT(x0);
ASSERT(x);
ASSERT(p);
@@ -376,8 +376,8 @@ void _dogleg_testGradient(unsigned int var, const double* p0,
}
else
{
- J_dense = malloc( Nmeas * Nstate * sizeof(J_dense[0]) );
- J_dense0 = malloc( Nmeas * Nstate * sizeof(J_dense[0]) );
+ J_dense = static_cast<double*>(malloc( Nmeas * Nstate * sizeof(J_dense[0]) ));
+ J_dense0 = static_cast<double*>(malloc( Nmeas * Nstate * sizeof(J_dense[0]) ));
dogleg_callback_dense_t* f_dense = (dogleg_callback_dense_t*)f;
p[var] -= GRADTEST_DELTA/2.0;
@@ -487,11 +487,13 @@ static void computeCauchyUpdate(dogleg_operatingPoint_t* point,
// LAPACK prototypes for a packed cholesky factorization and a linear solve
// using that factorization, respectively
+extern "C" {
int dpptrf_(char* uplo, int* n, double* ap,
int* info, int uplo_len);
int dpptrs_(char* uplo, int* n, int* nrhs,
double* ap, double* b, int* ldb, int* info,
int uplo_len);
+}
void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx)
@@ -538,8 +540,8 @@ void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solve
if(ctx->factorization_dense == NULL)
{
// Need to store symmetric JtJ, so I only need one triangle of it
- ctx->factorization_dense = malloc( ctx->Nstate * (ctx->Nstate+1) / 2 *
- sizeof( ctx->factorization_dense[0]));
+ ctx->factorization_dense = static_cast<double*>(malloc( ctx->Nstate * (ctx->Nstate+1) / 2 *
+ sizeof( ctx->factorization_dense[0])));
ASSERT(ctx->factorization_dense);
}
@@ -582,7 +584,9 @@ void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solve
int info;
- dpptrf_(&(char){'L'}, &(int){ctx->Nstate}, ctx->factorization_dense,
+ char uplo = 'L';
+ int Nstate_copy = ctx->Nstate;
+ dpptrf_(&uplo, &Nstate_copy, ctx->factorization_dense,
&info, 1);
ASSERT(info >= 0); // we MUST either succeed or see complain of singular
// JtJ
@@ -611,10 +615,10 @@ static void computeGaussNewtonUpdate(dogleg_operatingPoint_t* point, dogleg_solv
if( ctx->is_sparse )
{
// solve JtJ*updateGN = Jt*x. Gauss-Newton step is then -updateGN
- cholmod_dense Jt_x_dense = {.nrow = ctx->Nstate,
+ cholmod_dense Jt_x_dense = {.nrow = static_cast<size_t>(ctx->Nstate),
.ncol = 1,
- .nzmax = ctx->Nstate,
- .d = ctx->Nstate,
+ .nzmax = static_cast<size_t>(ctx->Nstate),
+ .d = static_cast<size_t>(ctx->Nstate),
.x = point->Jt_x,
.xtype = CHOLMOD_REAL,
.dtype = CHOLMOD_DOUBLE};
@@ -626,18 +630,22 @@ static void computeGaussNewtonUpdate(dogleg_operatingPoint_t* point, dogleg_solv
ctx->factorization,
&Jt_x_dense,
&ctx->common);
- vec_negate(point->updateGN_cholmoddense->x,
+ vec_negate(static_cast<double*>(point->updateGN_cholmoddense->x),
ctx->Nstate); // should be more efficient than this later
- point->updateGN_lensq = norm2(point->updateGN_cholmoddense->x, ctx->Nstate);
+ point->updateGN_lensq = norm2(static_cast<double*>(point->updateGN_cholmoddense->x), ctx->Nstate);
}
else
{
memcpy( point->updateGN_dense, point->Jt_x, ctx->Nstate * sizeof(point->updateGN_dense[0]));
int info;
- dpptrs_(&(char){'L'}, &(int){ctx->Nstate}, &(int){1},
+ char uplo = 'L';
+ int nhrs = 1;
+ int Nstate_copy = ctx->Nstate;
+ int Nstate_copy2 = ctx->Nstate;
+ dpptrs_(&uplo, &Nstate_copy, &nhrs,
ctx->factorization_dense,
- point->updateGN_dense, &(int){ctx->Nstate}, &info, 1);
+ point->updateGN_dense, &Nstate_copy2, &info, 1);
vec_negate(point->updateGN_dense,
ctx->Nstate); // should be more efficient than this later
@@ -677,7 +685,7 @@ static void computeInterpolatedUpdate(double* update_dogleg,
double dsq = trustregion*trustregion;
double norm2a = point->updateCauchy_lensq;
const double* a = point->updateCauchy;
- const double* b = ctx->is_sparse ? point->updateGN_cholmoddense->x : point->updateGN_dense;
+ const double* b = ctx->is_sparse ? static_cast<double*>(point->updateGN_cholmoddense->x) : point->updateGN_dense;
double l2 = 0.0;
double neg_c = 0.0;
@@ -1029,7 +1037,7 @@ dogleg_operatingPoint_t* allocOperatingPoint(int Nstate, int numMeasurements,
{
int is_sparse = NJnnz > 0;
- dogleg_operatingPoint_t* point = malloc(sizeof(dogleg_operatingPoint_t));
+ dogleg_operatingPoint_t* point = static_cast<dogleg_operatingPoint_t*>(malloc(sizeof(dogleg_operatingPoint_t)));
ASSERT(point != NULL);
@@ -1042,7 +1050,7 @@ dogleg_operatingPoint_t* allocOperatingPoint(int Nstate, int numMeasurements,
if(!is_sparse)
Npool += numMeasurements*Nstate + Nstate;
- double* pool = malloc( Npool * sizeof(double) );
+ double* pool = static_cast<double*>(malloc( Npool * sizeof(double) ));
ASSERT( pool != NULL );
point->p = &pool[0];
@@ -1170,7 +1178,7 @@ static double _dogleg_optimize(double* p, unsigned int Nstate,
int is_sparse = NJnnz > 0;
- dogleg_solverContext_t* ctx = malloc(sizeof(dogleg_solverContext_t));
+ dogleg_solverContext_t* ctx = static_cast<dogleg_solverContext_t*>(malloc(sizeof(dogleg_solverContext_t)));
ctx->f = f;
ctx->cookie = cookie;
ctx->factorization = NULL;
@@ -1294,10 +1302,13 @@ static bool pseudoinverse_J_dense(// output
memcpy(out,
&point->J_dense[i_meas0*ctx->Nstate],
NmeasInChunk*ctx->Nstate*sizeof(double));
- dpptrs_(&(char){'L'}, &(int){ctx->Nstate}, &NmeasInChunk,
+ char uplo = 'L';
+ int Nstate_copy = ctx->Nstate;
+ int Nstate_copy2 = ctx->Nstate;
+ dpptrs_(&uplo, &Nstate_copy, &NmeasInChunk,
ctx->factorization_dense,
out,
- &(int){ctx->Nstate}, &info, 1);
+ &Nstate_copy2, &info, 1);
return info==0;
}
@@ -1873,11 +1884,12 @@ static bool getOutliernessFactors_dense( // output
int Nmeasurements = ctx->Nmeasurements;
bool result = false;
- double* invJtJ_Jt = malloc(Nstate*chunk_size*sizeof(double));
+ double* invJtJ_Jt = static_cast<double*>(malloc(Nstate*chunk_size*sizeof(double)));
if(invJtJ_Jt == NULL)
{
SAY("Couldn't allocate invJtJ_Jt!");
- goto done;
+ free(invJtJ_Jt);
+ return result;
}
getOutliernessScale(scale,
@@ -1895,7 +1907,8 @@ static bool getOutliernessFactors_dense( // output
if(!pinvresult)
{
SAY("Couldn't compute pinv!");
- goto done;
+ free(invJtJ_Jt);
+ return result;
}
i_measurement_valid_chunk_start = i_measurement;
i_measurement_valid_chunk_last = i_measurement+chunk_size-1;
@@ -1926,7 +1939,6 @@ static bool getOutliernessFactors_dense( // output
}
result = true;
- done:
free(invJtJ_Jt);
return result;
}
@@ -1975,7 +1987,9 @@ static bool getOutliernessFactors_sparse( // output
if(!Jt_chunk)
{
SAY("Couldn't allocate Jt_chunk!");
- goto done;
+ if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common);
+ if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common);
+ return result;
}
getOutliernessScale(scale,
@@ -1995,7 +2009,9 @@ static bool getOutliernessFactors_sparse( // output
if(invJtJ_Jt == NULL)
{
SAY("Couldn't compute pinv!");
- goto done;
+ if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common);
+ if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common);
+ return result;
}
i_measurement_valid_chunk_start = i_measurement;
@@ -2032,7 +2048,6 @@ static bool getOutliernessFactors_sparse( // output
}
result = true;
- done:
if(Jt_chunk) cholmod_free_dense(&Jt_chunk, &ctx->common);
if(invJtJ_Jt) cholmod_free_dense(&invJtJ_Jt, &ctx->common);
return result;
@@ -2222,18 +2237,18 @@ double dogleg_getOutliernessTrace_newFeature_sparse(const double* Jqu
for(int j=0; j<NstateActive; j++)
Jt_i[j + i*NstateActive] = istateActive + j;
}
- cholmod_sparse Jt_query_sparse = {.nrow = ctx->Nstate,
- .ncol = featureSize,
- .nzmax = NstateActive*featureSize,
- .p = (void*)Jt_p.data(),
- .i = (void*)Jt_i.data(),
- .x = (double*)JqueryFeature,
- .sorted = 1,
- .packed = 1,
+ cholmod_sparse Jt_query_sparse = {.nrow = static_cast<size_t>(ctx->Nstate),
+ .ncol = static_cast<size_t>(featureSize),
+ .nzmax = static_cast<size_t>(NstateActive*featureSize),
+ .p = Jt_p.data(),
+ .i = Jt_i.data(),
+ .x = const_cast<double*>(JqueryFeature),
.stype = 0, // NOT symmetric
.itype = CHOLMOD_INT,
.xtype = CHOLMOD_REAL,
- .dtype = CHOLMOD_DOUBLE};
+ .dtype = CHOLMOD_DOUBLE,
+ .sorted = 1,
+ .packed = 1};
// Really shouldn't need to do this every time. In fact I probably don't need
// to do it at all, since this will have been done by the solver during the
@@ -2408,18 +2423,22 @@ bool dogleg_markOutliers(// output, input
bool markedAny = false;
- double* factors = malloc(Nfeatures * sizeof(double));
+ double* factors = static_cast<double*>(malloc(Nfeatures * sizeof(double)));
if(factors == NULL)
{
SAY("Error allocating factors");
- goto done;
+ free(factors);
+ return markedAny;
}
if(!dogleg_getOutliernessFactors(factors, scale,
featureSize, Nfeatures,
*Noutliers,
point, ctx))
- goto done;
+ {
+ free(factors);
+ return markedAny;
+ }
// I have my list of POTENTIAL outliers (any that have factor > 1.0). I
// check to see how much confidence I would lose if I were to throw out any
@@ -2427,7 +2446,10 @@ bool dogleg_markOutliers(// output, input
// is acceptable
double confidence0 = getConfidence(-1);
if( confidence0 < 0.0 )
- goto done;
+ {
+ free(factors);
+ return markedAny;
+ }
SAY_IF_VERBOSE("Initial confidence: %g", confidence0);
@@ -2466,7 +2488,6 @@ bool dogleg_markOutliers(// output, input
}
}
- done:
free(factors);
return markedAny;
}
@@ -2490,11 +2511,11 @@ void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude),
if(featureSize <= 1)
featureSize = 1;
- double* factors = malloc(Nfeatures * sizeof(double));
+ double* factors = static_cast<double*>(malloc(Nfeatures * sizeof(double)));
if(factors == NULL)
{
SAY("Error allocating factors");
- goto done;
+ free(factors);
}
dogleg_getOutliernessFactors(factors, scale,
@@ -2516,6 +2537,5 @@ void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude),
rot_confidence_drop_relative);
}
- done:
free(factors);
}

65
upstream_utils/mrcal.py Executable file
View File

@@ -0,0 +1,65 @@
#!/usr/bin/env python3
import os
import shutil
from upstream_utils import Lib, walk_cwd_and_copy_if
def copy_upstream_src(wpilib_root):
wpical = os.path.join(wpilib_root, "wpical")
# Delete old install
for d in [
"src/main/native/thirdparty/mrcal/src",
"src/main/native/thirdparty/mrcal/include",
]:
shutil.rmtree(os.path.join(wpical, d), ignore_errors=True)
files = walk_cwd_and_copy_if(
lambda dp, f: (f.endswith(".h") or f.endswith(".hh"))
and not f.endswith("heap.h")
and not f.endswith("stereo-matching-libelas.h")
and not dp.startswith(os.path.join(".", "test")),
os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"),
)
files = files + walk_cwd_and_copy_if(
lambda dp, f: (
f.endswith(".c")
or f.endswith(".cc")
or f.endswith(".cpp")
or f.endswith(".pl")
)
and not f.endswith("heap.cc")
and not f.endswith("mrcal-pywrap.c")
and not f.endswith("image.c")
and not f.endswith("stereo.c")
and not f.endswith("stereo-matching-libelas.cc")
and not f.endswith("uncertainty.c")
and not f.endswith("traverse-sensor-links.c")
and not dp.startswith(os.path.join(".", "doc"))
and not dp.startswith(os.path.join(".", "test")),
os.path.join(wpical, "src/main/native/thirdparty/mrcal/src"),
)
for f in files:
with open(f) as file:
content = file.read()
content = content.replace("#warning", "// #warning")
content = content.replace('__attribute__ ((visibility ("hidden")))', "")
content = content.replace("__attribute__((unused))", "")
with open(f, "w") as file:
file.write(content)
def main():
name = "mrcal"
url = "https://github.com/dkogan/mrcal"
tag = "662a539d3cbba4948c31d06a780569173b3fb6e6"
mrcal = Lib(name, url, tag, copy_upstream_src)
mrcal.main()
if __name__ == "__main__":
main()

88
upstream_utils/mrcal_java.py Executable file
View File

@@ -0,0 +1,88 @@
#!/usr/bin/env python3
import os
import shutil
from upstream_utils import Lib, walk_cwd_and_copy_if
def delete_lines_by_range(file_path, start_line, end_line):
# Read all lines from the file
with open(file_path, "r") as file:
lines = file.readlines()
# Filter out lines that are within the specified range
filtered_lines = [
line
for i, line in enumerate(lines, start=1)
if not (start_line <= i <= end_line)
]
# Write the remaining lines back to the file
with open(file_path, "w") as file:
file.writelines(filtered_lines)
def copy_upstream_src(wpilib_root):
wpical = os.path.join(wpilib_root, "wpical")
# Delete old install
for d in [
"src/main/native/thirdparty/mrcal_java/src",
"src/main/native/thirdparty/mrcal_java/include",
]:
shutil.rmtree(os.path.join(wpical, d), ignore_errors=True)
os.chdir("src")
files = walk_cwd_and_copy_if(
lambda dp, f: f.endswith("mrcal_wrapper.h"),
os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/include"),
)
files = walk_cwd_and_copy_if(
lambda dp, f: f.endswith("mrcal_wrapper.cpp"),
os.path.join(wpical, "src/main/native/thirdparty/mrcal_java/src"),
)
for f in files:
with open(f) as file:
content = file.read()
content = content.replace("#include <malloc.h>", "")
content = content.replace(
"// mrcal_point3_t *c_observations_point_pool = observations_point;",
"mrcal_point3_t *c_observations_point_pool = observations_point;",
)
content = content.replace(
"// mrcal_observation_point_triangulated_t *observations_point_triangulated =",
"mrcal_observation_point_triangulated_t *observations_point_triangulated = NULL;",
)
content = content.replace(
"// observations_point_triangulated,",
"observations_point_triangulated,",
)
content = content.replace(
"// 0, // hard-coded to 0", "0, // hard-coded to 0"
)
content = content.replace(
"// observations_point_triangulated, -1,",
"observations_point_triangulated, -1,",
)
content = content.replace(
"c_observations_board_pool, &mrcal_lensmodel, c_imagersizes,",
"c_observations_board_pool, c_observations_point_pool, &mrcal_lensmodel, c_imagersizes,",
)
with open(f, "w") as file:
file.write(content)
def main():
name = "mrcal_java"
url = "https://github.com/PhotonVision/mrcal-java"
tag = "5f9d3168ccf1ecdfca48da13ea07fffa47f95d00"
mrcal_java = Lib(name, url, tag, copy_upstream_src)
mrcal_java.main()
if __name__ == "__main__":
main()

22
upstream_utils/mrcal_java_patches/0001-Support-new-mrcal-version.patch Normal file
View File

@@ -0,0 +1,22 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Sat, 30 Nov 2024 17:52:37 -0500
Subject: [PATCH] Support new mrcal version
---
src/mrcal_wrapper.cpp | 2 +-
1 file changed, 1 insertion(+), 1 deletion(-)
diff --git a/src/mrcal_wrapper.cpp b/src/mrcal_wrapper.cpp
index 569f5928ba0b389e40476cbfc224a0c09ecd5814..3a9a5519e670b0c867281f1e05aa2046b938fa8d 100644
--- a/src/mrcal_wrapper.cpp
+++ b/src/mrcal_wrapper.cpp
@@ -212,7 +212,7 @@ static std::unique_ptr<mrcal_result> mrcal_calibrate(
std::vector<double> residuals = {c_x_final, c_x_final + Nmeasurements};
return std::make_unique<mrcal_result>(
true, intrinsics, stats.rms_reproj_error__pixels, residuals,
- calobject_warp, stats.Noutliers);
+ calobject_warp, stats.Noutliers_board);
}
struct MrcalSolveOptions {

155
upstream_utils/mrcal_patches/0001-Fix-MSVC-build-errors.patch Normal file
View File

@@ -0,0 +1,155 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Mon, 11 Nov 2024 00:15:05 -0500
Subject: [PATCH 1/8] Fix MSVC build errors
---
autodiff.hh | 4 ++--
mrcal-internal.h | 22 +++++++++++-----------
mrcal-types.h | 26 ++++++++++----------------
mrcal.c | 9 ---------
4 files changed, 23 insertions(+), 38 deletions(-)
diff --git a/autodiff.hh b/autodiff.hh
index b6f217354c9de45493d50f776082fe2151330a1d..37aa8c5a048a538ccdf500a43bbd8d788c953a05 100644
--- a/autodiff.hh
+++ b/autodiff.hh
@@ -26,7 +26,7 @@ template<int NGRAD>
struct val_withgrad_t
{
double x;
- double j[NGRAD];
+ double j[NGRAD == 0 ? 1 : NGRAD];
__attribute__ ((visibility ("hidden")))
val_withgrad_t(double _x = 0.0) : x(_x)
@@ -281,7 +281,7 @@ struct val_withgrad_t
template<int NGRAD, int NVEC>
struct vec_withgrad_t
{
- val_withgrad_t<NGRAD> v[NVEC];
+ val_withgrad_t<NGRAD> v[NVEC == 0 ? 1 : NVEC];
vec_withgrad_t() {}
diff --git a/mrcal-internal.h b/mrcal-internal.h
index 4ec6acadb5fc92ec264fb460e1fcc882dd3face3..8fc2857758ef7763cc1ef5b79a495aec7b7bc1bd 100644
--- a/mrcal-internal.h
+++ b/mrcal-internal.h
@@ -12,16 +12,16 @@
// wrapper only
// These models have no precomputed data
-typedef struct {} mrcal_LENSMODEL_PINHOLE__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_LONLAT__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_LATLON__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_OPENCV4__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_OPENCV5__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_OPENCV8__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_OPENCV12__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_CAHVOR__precomputed_t;
-typedef struct {} mrcal_LENSMODEL_CAHVORE__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_CAHVORE__precomputed_t;
// The splined stereographic models configuration parameters can be used to
// compute the segment size. I cache this computation
@@ -38,7 +38,7 @@ typedef struct
union
{
#define PRECOMPUTED_STRUCT(s,n) mrcal_ ##s##__precomputed_t s##__precomputed;
- MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT);
+ MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT)
#undef PRECOMPUTED_STRUCT
};
} mrcal_projection_precomputed_t;
diff --git a/mrcal-types.h b/mrcal-types.h
index e5cf6c637f8f2bf9e4bbe4ad443661159c511887..7e43319b882c3e29eb3429f95c77fc8bd664ce1a 100644
--- a/mrcal-types.h
+++ b/mrcal-types.h
@@ -45,24 +45,18 @@
// parametric models have no extra configuration
-typedef struct {} mrcal_LENSMODEL_PINHOLE__config_t;
-typedef struct {} mrcal_LENSMODEL_STEREOGRAPHIC__config_t;
-typedef struct {} mrcal_LENSMODEL_LONLAT__config_t;
-typedef struct {} mrcal_LENSMODEL_LATLON__config_t;
-typedef struct {} mrcal_LENSMODEL_OPENCV4__config_t;
-typedef struct {} mrcal_LENSMODEL_OPENCV5__config_t;
-typedef struct {} mrcal_LENSMODEL_OPENCV8__config_t;
-typedef struct {} mrcal_LENSMODEL_OPENCV12__config_t;
-typedef struct {} mrcal_LENSMODEL_CAHVOR__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__config_t;
#define _MRCAL_ITEM_DEFINE_ELEMENT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) type name bitfield;
-#ifndef __cplusplus
-// This barfs with g++ 4.8, so I disable it for C++ in general. Checking it for
-// C code is sufficient
-_Static_assert(sizeof(uint16_t) == sizeof(unsigned short int), "I need a short to be 16-bit. Py_BuildValue doesn't let me just specify that. H means 'unsigned short'");
-#endif
-
// Configuration for CAHVORE. These are given as an an
// "X macro": https://en.wikipedia.org/wiki/X_Macro
#define MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_, cookie) \
@@ -128,7 +122,7 @@ typedef struct
union
{
#define CONFIG_STRUCT(s,n) mrcal_ ##s##__config_t s##__config;
- MRCAL_LENSMODEL_LIST(CONFIG_STRUCT);
+ MRCAL_LENSMODEL_LIST(CONFIG_STRUCT)
#undef CONFIG_STRUCT
};
} mrcal_lensmodel_t;
diff --git a/mrcal.c b/mrcal.c
index d1195a36050e5cbde393ed033e4d5699ce52225a..85ab4e26a5ab487fc13e66ebd486bd7c00480c05 100644
--- a/mrcal.c
+++ b/mrcal.c
@@ -2674,8 +2674,6 @@ void project( // out
if(!camera_at_identity)
{
// make sure I can pass mrcal_pose_t.r as an rt[] transformation
- _Static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure");
- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure");
mrcal_compose_rt( _joint_rt,
gg._d_rj_rc, gg._d_rj_rf,
gg._d_tj_rc, gg._d_tj_tf,
@@ -3465,8 +3463,6 @@ void mrcal_pack_solver_state_vector( // out, in
lensmodel, problem_selections,
Ncameras_intrinsics );
- _Static_assert( offsetof(mrcal_pose_t, r) == 0, "mrcal_pose_t has expected structure");
- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double), "mrcal_pose_t has expected structure");
if( problem_selections.do_optimize_extrinsics )
for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++)
{
@@ -3715,11 +3711,6 @@ void mrcal_unpack_solver_state_vector( // out, in
if( problem_selections.do_optimize_extrinsics )
{
- _Static_assert( offsetof(mrcal_pose_t, r) == 0,
- "mrcal_pose_t has expected structure");
- _Static_assert( offsetof(mrcal_pose_t, t) == 3*sizeof(double),
- "mrcal_pose_t has expected structure");
-
mrcal_pose_t* extrinsics_fromref = (mrcal_pose_t*)(&b[i_state]);
for(int icam_extrinsics=0; icam_extrinsics < Ncameras_extrinsics; icam_extrinsics++)
i_state += unpack_solver_state_extrinsics_one( &extrinsics_fromref[icam_extrinsics], &b[i_state] );

34
upstream_utils/mrcal_patches/0002-Make-mrcal-functions-extern-C.patch Normal file
View File

@@ -0,0 +1,34 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Fri, 22 Nov 2024 09:54:00 -0500
Subject: [PATCH 2/8] Make mrcal functions extern "C"
---
mrcal.h | 7 ++++++-
1 file changed, 6 insertions(+), 1 deletion(-)
diff --git a/mrcal.h b/mrcal.h
index 1eef334a79597cfae61f8111ab97ca04f02b83b2..488ddfe72b3407b3fcb7c3d9de43d1a07433744d 100644
--- a/mrcal.h
+++ b/mrcal.h
@@ -20,7 +20,9 @@
////////////////////////////////////////////////////////////////////////////////
//////////////////// Lens models
////////////////////////////////////////////////////////////////////////////////
-
+#ifdef __cplusplus
+extern "C" {
+#endif
// Return an array of strings listing all the available lens models
//
// These are all "unconfigured" strings that use "..." placeholders for any
@@ -846,6 +848,9 @@ void mrcal_free_cameramodel(mrcal_cameramodel_t** cameramodel);
bool mrcal_write_cameramodel_file(const char* filename,
const mrcal_cameramodel_t* cameramodel);
+#ifdef __cplusplus
+} // extern "C"
+#endif
#define DECLARE_mrcal_apply_color_map(T,Tname) \
bool mrcal_apply_color_map_##Tname( \
mrcal_image_bgr_t* out, \

14
upstream_utils/mrcal_patches/0003-Make-mrcal-C.patch Normal file
View File

@@ -0,0 +1,14 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Fri, 22 Nov 2024 09:55:22 -0500
Subject: [PATCH 3/8] Make mrcal C++
---
mrcal.c => mrcal.cpp | 0
1 file changed, 0 insertions(+), 0 deletions(-)
rename mrcal.c => mrcal.cpp (100%)
diff --git a/mrcal.c b/mrcal.cpp
similarity index 100%
rename from mrcal.c
rename to mrcal.cpp

284
upstream_utils/mrcal_patches/0004-Replace-VLAs-with-vectors.patch Normal file
View File

@@ -0,0 +1,284 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Fri, 29 Nov 2024 22:56:02 -0500
Subject: [PATCH 4/8] Replace VLAs with vectors
---
mrcal.cpp | 101 +++++++++++++++++++++++++++---------------------------
1 file changed, 51 insertions(+), 50 deletions(-)
diff --git a/mrcal.cpp b/mrcal.cpp
index 85ab4e26a5ab487fc13e66ebd486bd7c00480c05..5f28f3303da4b4669af896587427b2ad3ad4b7e6 100644
--- a/mrcal.cpp
+++ b/mrcal.cpp
@@ -7,6 +7,7 @@
// http://www.apache.org/licenses/LICENSE-2.0
// Apparently I need this in MSVC to get constants
+#include <vector>
#define _USE_MATH_DEFINES
#include <stdio.h>
@@ -28,7 +29,7 @@
#define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0)
-
+#define restrict __restrict
// Returns a static string, using "..." as a placeholder for any configuration
// values
@@ -1141,7 +1142,7 @@ void project_cahvor( // outputs
double s__dmu_dalphabeta__domega_dalphabeta = dmu_dtau * s__dtau_dalphabeta__domega_dalphabeta;
double dpdistorted_dpcam[3*3] = {};
- double dpdistorted_ddistortion[3*NdistortionParams];
+ std::vector<double> dpdistorted_ddistortion(3*NdistortionParams);
for(int i=0; i<3; i++)
{
@@ -2918,7 +2919,7 @@ bool _mrcal_project_internal( // out
.t = p[i]};
// simple non-intrinsics-gradient path
- double dq_dintrinsics_pool_double[Ngradients];
+ std::vector<double> dq_dintrinsics_pool_double(Ngradients);
int dq_dintrinsics_pool_int [1];
double* dq_dfxy = NULL;
double* dq_dintrinsics_nocore = NULL;
@@ -2926,7 +2927,7 @@ bool _mrcal_project_internal( // out
project( &q[i],
- dq_dintrinsics_pool_double,
+ dq_dintrinsics_pool_double.data(),
dq_dintrinsics_pool_int,
&dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta,
@@ -3943,21 +3944,21 @@ bool mrcal_corresponding_icam_extrinsics(// out
return false;
}
- int icam_map_to_extrinsics[Ncameras_intrinsics];
- int icam_map_to_intrinsics[Ncameras_extrinsics+1];
+ std::vector<int> icam_map_to_extrinsics(Ncameras_intrinsics);
+ std::vector<int> icam_map_to_intrinsics(Ncameras_extrinsics+1);
for(int i=0; i<Ncameras_intrinsics; i++) icam_map_to_extrinsics[i] = -100;
for(int i=0; i<Ncameras_extrinsics+1; i++) icam_map_to_intrinsics[i] = -100;
for(int i=0; i<Nobservations_board; i++)
if(!_corresponding_icam_extrinsics__check( &observations_board[i].icam, i,
- icam_map_to_extrinsics,
- icam_map_to_intrinsics,
+ icam_map_to_extrinsics.data(),
+ icam_map_to_intrinsics.data(),
"board"))
return false;
for(int i=0; i<Nobservations_point; i++)
if(!_corresponding_icam_extrinsics__check( &observations_point[i].icam, i,
- icam_map_to_extrinsics,
- icam_map_to_intrinsics,
+ icam_map_to_extrinsics.data(),
+ icam_map_to_intrinsics.data(),
"point"))
return false;
@@ -4556,8 +4557,8 @@ void optimizer_callback(// input state
// 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];
+ std::vector<std::vector<double>> intrinsics_all(ctx->Ncameras_intrinsics, std::vector<double>(ctx->Nintrinsics));
+ std::vector<mrcal_pose_t> camera_rt(ctx->Ncameras_extrinsics);
mrcal_calobject_warp_t calobject_warp_local = {};
const int i_var_calobject_warp =
@@ -4669,12 +4670,12 @@ void optimizer_callback(// input state
// these are computed in respect to the real-unit parameters,
// NOT the unit-scale parameters used by the optimizer
- mrcal_point3_t dq_drcamera [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
- mrcal_point3_t dq_dtcamera [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
- mrcal_point3_t dq_drframe [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
- mrcal_point3_t dq_dtframe [ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
- mrcal_calobject_warp_t dq_dcalobject_warp[ctx->calibration_object_width_n*ctx->calibration_object_height_n][2];
- mrcal_point2_t q_hypothesis [ctx->calibration_object_width_n*ctx->calibration_object_height_n];
+ std::vector<mrcal_point3_t> dq_drcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<mrcal_point3_t> dq_dtcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<mrcal_point3_t> dq_drframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<mrcal_point3_t> dq_dtframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<mrcal_calobject_warp_t> dq_dcalobject_warp(ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<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.
@@ -4686,35 +4687,35 @@ void optimizer_callback(// input state
// this case explicitly here. I store dx/dfx and dy/dfy; no cross terms
int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics);
- double dq_dintrinsics_pool_double[ctx->calibration_object_width_n*ctx->calibration_object_height_n*Ngradients];
- int dq_dintrinsics_pool_int [ctx->calibration_object_width_n*ctx->calibration_object_height_n];
+ std::vector<double> dq_dintrinsics_pool_double(ctx->calibration_object_width_n*ctx->calibration_object_height_n*Ngradients);
+ std::vector<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,
+ project(q_hypothesis.data(),
ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ?
- dq_dintrinsics_pool_double : NULL,
+ dq_dintrinsics_pool_double.data() : NULL,
ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ?
- dq_dintrinsics_pool_int : NULL,
+ dq_dintrinsics_pool_int.data() : NULL,
&dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta,
ctx->problem_selections.do_optimize_extrinsics ?
- (mrcal_point3_t*)dq_drcamera : NULL,
+ dq_drcamera.data() : NULL,
ctx->problem_selections.do_optimize_extrinsics ?
- (mrcal_point3_t*)dq_dtcamera : NULL,
+ dq_dtcamera.data() : NULL,
ctx->problem_selections.do_optimize_frames ?
- (mrcal_point3_t*)dq_drframe : NULL,
+ dq_drframe.data() : NULL,
ctx->problem_selections.do_optimize_frames ?
- (mrcal_point3_t*)dq_dtframe : NULL,
+ dq_dtframe.data() : NULL,
has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) ?
- (mrcal_calobject_warp_t*)dq_dcalobject_warp : NULL,
+ dq_dcalobject_warp.data() : NULL,
// input
- intrinsics_all[icam_intrinsics],
+ intrinsics_all[icam_intrinsics].data(),
&camera_rt[icam_extrinsics], &frame_rt,
ctx->calobject_warp == NULL ? NULL : &calobject_warp_local,
icam_extrinsics < 0,
@@ -4800,43 +4801,43 @@ void optimizer_callback(// input state
if( icam_extrinsics >= 0 )
{
STORE_JACOBIAN3( i_var_camera_rt + 0,
- dq_drcamera[i_pt][i_xy].xyz[0] *
+ dq_drcamera[i_pt * 2 + i_xy].xyz[0] *
weight * SCALE_ROTATION_CAMERA,
- dq_drcamera[i_pt][i_xy].xyz[1] *
+ dq_drcamera[i_pt * 2 + i_xy].xyz[1] *
weight * SCALE_ROTATION_CAMERA,
- dq_drcamera[i_pt][i_xy].xyz[2] *
+ dq_drcamera[i_pt * 2 + i_xy].xyz[2] *
weight * SCALE_ROTATION_CAMERA);
STORE_JACOBIAN3( i_var_camera_rt + 3,
- dq_dtcamera[i_pt][i_xy].xyz[0] *
+ dq_dtcamera[i_pt * 2 + i_xy].xyz[0] *
weight * SCALE_TRANSLATION_CAMERA,
- dq_dtcamera[i_pt][i_xy].xyz[1] *
+ dq_dtcamera[i_pt * 2 + i_xy].xyz[1] *
weight * SCALE_TRANSLATION_CAMERA,
- dq_dtcamera[i_pt][i_xy].xyz[2] *
+ dq_dtcamera[i_pt * 2 + i_xy].xyz[2] *
weight * SCALE_TRANSLATION_CAMERA);
}
if( ctx->problem_selections.do_optimize_frames )
{
STORE_JACOBIAN3( i_var_frame_rt + 0,
- dq_drframe[i_pt][i_xy].xyz[0] *
+ dq_drframe[i_pt * 2 + i_xy].xyz[0] *
weight * SCALE_ROTATION_FRAME,
- dq_drframe[i_pt][i_xy].xyz[1] *
+ dq_drframe[i_pt * 2 + i_xy].xyz[1] *
weight * SCALE_ROTATION_FRAME,
- dq_drframe[i_pt][i_xy].xyz[2] *
+ dq_drframe[i_pt * 2 + i_xy].xyz[2] *
weight * SCALE_ROTATION_FRAME);
STORE_JACOBIAN3( i_var_frame_rt + 3,
- dq_dtframe[i_pt][i_xy].xyz[0] *
+ dq_dtframe[i_pt * 2 + i_xy].xyz[0] *
weight * SCALE_TRANSLATION_FRAME,
- dq_dtframe[i_pt][i_xy].xyz[1] *
+ dq_dtframe[i_pt * 2 + i_xy].xyz[1] *
weight * SCALE_TRANSLATION_FRAME,
- dq_dtframe[i_pt][i_xy].xyz[2] *
+ dq_dtframe[i_pt * 2 + i_xy].xyz[2] *
weight * SCALE_TRANSLATION_FRAME);
}
if( has_calobject_warp(ctx->problem_selections,ctx->Nobservations_board) )
{
STORE_JACOBIAN_N( i_var_calobject_warp,
- dq_dcalobject_warp[i_pt][i_xy].values,
+ dq_dcalobject_warp[i_pt * 2 + i_xy].values,
weight * SCALE_CALOBJECT_WARP,
MRCAL_NSTATE_CALOBJECT_WARP);
}
@@ -5061,7 +5062,7 @@ void optimizer_callback(// input state
int Ngradients = get_Ngradients(&ctx->lensmodel, ctx->Nintrinsics);
// WARNING: "compute size(dq_dintrinsics_pool_double) correctly and maybe bounds-check"
- double dq_dintrinsics_pool_double[Ngradients];
+ std::vector<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];
@@ -5081,7 +5082,7 @@ void optimizer_callback(// input state
project(&q_hypothesis,
ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ?
- dq_dintrinsics_pool_double : NULL,
+ dq_dintrinsics_pool_double.data() : NULL,
ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ?
dq_dintrinsics_pool_int : NULL,
&dq_dfxy, &dq_dintrinsics_nocore, &gradient_sparse_meta,
@@ -5095,7 +5096,7 @@ void optimizer_callback(// input state
NULL,
// input
- intrinsics_all[icam_intrinsics],
+ intrinsics_all[icam_intrinsics].data(),
&camera_rt[icam_extrinsics],
// I only have the point position, so the 'rt' memory
@@ -6590,8 +6591,8 @@ mrcal_optimize( // out
// WARNING: is it reasonable to put this on the stack? Can I use
// b_packed_final for this?
- double packed_state[Nstate];
- pack_solver_state(packed_state,
+ std::vector<double> packed_state(Nstate);
+ pack_solver_state(packed_state.data(),
lensmodel, intrinsics,
extrinsics_fromref,
frames_toref,
@@ -6628,7 +6629,7 @@ mrcal_optimize( // out
if(solver_context != NULL)
dogleg_freeContext(&solver_context);
- norm2_error = dogleg_optimize2(packed_state,
+ norm2_error = dogleg_optimize2(packed_state.data(),
Nstate, ctx.Nmeasurements, ctx.N_j_nonzero,
(dogleg_callback_t*)&optimizer_callback, &ctx,
&dogleg_parameters,
@@ -6683,7 +6684,7 @@ mrcal_optimize( // out
frames_toref, // Nframes of these
points, // Npoints of these
calobject_warp,
- packed_state,
+ packed_state.data(),
lensmodel,
problem_selections,
Ncameras_intrinsics, Ncameras_extrinsics,
@@ -6793,7 +6794,7 @@ mrcal_optimize( // out
}
else
for(int ivar=0; ivar<Nstate; ivar++)
- dogleg_testGradient(ivar, packed_state,
+ dogleg_testGradient(ivar, packed_state.data(),
Nstate, ctx.Nmeasurements, ctx.N_j_nonzero,
(dogleg_callback_t*)&optimizer_callback, &ctx);

27
upstream_utils/mrcal_patches/0005-Use-immediately-invoked-lambda.patch Normal file
View File

@@ -0,0 +1,27 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Fri, 29 Nov 2024 22:56:22 -0500
Subject: [PATCH 5/8] Use immediately invoked lambda
---
mrcal.cpp | 4 ++--
1 file changed, 2 insertions(+), 2 deletions(-)
diff --git a/mrcal.cpp b/mrcal.cpp
index 5f28f3303da4b4669af896587427b2ad3ad4b7e6..e048bef1d1b5addfd137d3c492108c36e56eda7f 100644
--- a/mrcal.cpp
+++ b/mrcal.cpp
@@ -6668,11 +6668,11 @@ mrcal_optimize( // out
solver_context->beforeStep->x,
extrinsics_fromref,
verbose) &&
- ({MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again",
+ ([=]{MSG("Threw out some outliers. New count = %d/%d (%.1f%%). Going again",
stats.Noutliers_board,
Nmeasurements_board,
(double)(stats.Noutliers_board * 100) / (double)Nmeasurements_board);
- true;}));
+ return true;}()));
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
#warning "triangulated-solve: the above print should deal with triangulated points too"
#endif

434
upstream_utils/mrcal_patches/0006-Fix-MSVC-build-errors.patch Normal file
View File

@@ -0,0 +1,434 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Wed, 4 Dec 2024 00:58:00 -0500
Subject: [PATCH 6/8] Fix MSVC build errors
---
minimath/minimath-extra.h | 1 +
mrcal.cpp | 57 ++++++-----
poseutils.c | 209 --------------------------------------
3 files changed, 32 insertions(+), 235 deletions(-)
diff --git a/minimath/minimath-extra.h b/minimath/minimath-extra.h
index 2930a5d6aa47af29a3255342838d679032ee4562..194b0ccf22c47a8d693eee24f6f563a2d651f338 100644
--- a/minimath/minimath-extra.h
+++ b/minimath/minimath-extra.h
@@ -5,6 +5,7 @@
// replacement, and I'm not going to be thorough and I'm not going to add tests
// until I do that.
+#define restrict __restrict
// Upper triangle is stored, in the usual row-major order.
__attribute__((unused))
diff --git a/mrcal.cpp b/mrcal.cpp
index e048bef1d1b5addfd137d3c492108c36e56eda7f..1560cd106ff5bcfe49476f785aa8108119318b3a 100644
--- a/mrcal.cpp
+++ b/mrcal.cpp
@@ -2742,11 +2742,10 @@ void project( // out
const mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t* config =
&lensmodel->LENSMODEL_SPLINED_STEREOGRAPHIC__config;
*gradient_sparse_meta =
- (gradient_sparse_meta_t)
{
- .run_side_length = config->order+1,
- .ivar_stridey = 2*config->Nx,
- .pool = &dq_dintrinsics_pool_double[ivar_pool]
+ &dq_dintrinsics_pool_double[ivar_pool],
+ static_cast<uint16_t>(config->order+1),
+ static_cast<uint16_t>(2*config->Nx),
};
ivar_pool += Npoints*2 * runlen;
}
@@ -2765,10 +2764,11 @@ void project( // out
if( calibration_object_width_n == 0 )
{ // projecting discrete points
+ mrcal_point3_t pt_ref = {}; // Unused
mrcal_point3_t p =
_propagate_extrinsics( _dp_drc,_dp_dtc,_dp_drf,_dp_dtf,
&dp_drc,&dp_dtc,&dp_drf,&dp_dtf,
- &(mrcal_point3_t){},
+ &pt_ref,
camera_at_identity ? NULL : &gg,
Rj, d_Rj_rj, &joint_rt[3]);
_project_point( q,
@@ -3227,10 +3227,11 @@ bool _mrcal_unproject_internal( // out
// MSG("init. q=(%g,%g)", q[i].x, q[i].y);
// initial estimate: pinhole projection
+ const mrcal_point3_t v = {.x = (q[i].x-cx)/fx,
+ .y = (q[i].y-cy)/fy,
+ .z = 1.};
mrcal_project_stereographic( (mrcal_point2_t*)out->xyz, NULL,
- &(mrcal_point3_t){.x = (q[i].x-cx)/fx,
- .y = (q[i].y-cy)/fy,
- .z = 1.},
+ &v,
1,
intrinsics );
// MSG("init. out->xyz[]=(%g,%g)", out->x, out->y);
@@ -4670,10 +4671,10 @@ void optimizer_callback(// input state
// these are computed in respect to the real-unit parameters,
// NOT the unit-scale parameters used by the optimizer
- std::vector<mrcal_point3_t> dq_drcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
- std::vector<mrcal_point3_t> dq_dtcamera_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
- std::vector<mrcal_point3_t> dq_drframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
- std::vector<mrcal_point3_t> dq_dtframe_vec (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<mrcal_point3_t> dq_drcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<mrcal_point3_t> dq_dtcamera (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<mrcal_point3_t> dq_drframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
+ std::vector<mrcal_point3_t> dq_dtframe (ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
std::vector<mrcal_calobject_warp_t> dq_dcalobject_warp(ctx->calibration_object_width_n*ctx->calibration_object_height_n*2);
std::vector<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.
@@ -6242,7 +6243,7 @@ bool mrcal_optimizer_callback(// out
problem_selections.do_optimize_extrinsics ) )
{
MSG("ERROR: We have triangulated points. At this time this is only allowed if we're NOT optimizing intrinsics AND if we ARE optimizing extrinsics.");
- goto done;
+ return result;
}
if( Nobservations_board > 0 )
@@ -6250,7 +6251,7 @@ bool mrcal_optimizer_callback(// out
if( problem_selections.do_optimize_calobject_warp && calobject_warp == NULL )
{
MSG("ERROR: We're optimizing the calibration object warp, so a buffer with a seed MUST be passed in.");
- goto done;
+ return result;
}
}
else
@@ -6266,7 +6267,7 @@ bool mrcal_optimizer_callback(// out
!problem_selections.do_optimize_calobject_warp)
{
MSG("Not optimizing any of our variables!");
- goto done;
+ return result;
}
@@ -6279,7 +6280,7 @@ bool mrcal_optimizer_callback(// out
{
MSG("The buffer passed to fill-in b_packed has the wrong size. Needed exactly %d bytes, but got %d bytes",
Nstate*(int)sizeof(double),buffer_size_b_packed);
- goto done;
+ return result;
}
int Nmeasurements = mrcal_num_measurements(Nobservations_board,
@@ -6312,7 +6313,7 @@ bool mrcal_optimizer_callback(// out
{
MSG("The buffer passed to fill-in x has the wrong size. Needed exactly %d bytes, but got %d bytes",
Nmeasurements*(int)sizeof(double),buffer_size_x);
- goto done;
+ return result;
}
const int Npoints_fromBoards =
@@ -6332,9 +6333,9 @@ bool mrcal_optimizer_callback(// out
.Npoints_fixed = Npoints_fixed,
.observations_board = observations_board,
.observations_board_pool = observations_board_pool,
- .observations_point_pool = observations_point_pool,
.Nobservations_board = Nobservations_board,
.observations_point = observations_point,
+ .observations_point_pool = observations_point_pool,
.Nobservations_point = Nobservations_point,
.observations_point_triangulated = observations_point_triangulated,
.Nobservations_point_triangulated = Nobservations_point_triangulated,
@@ -6367,7 +6368,6 @@ bool mrcal_optimizer_callback(// out
result = true;
-done:
return result;
}
@@ -6516,9 +6516,9 @@ mrcal_optimize( // out
.Npoints_fixed = Npoints_fixed,
.observations_board = observations_board,
.observations_board_pool = observations_board_pool,
- .observations_point_pool = observations_point_pool,
.Nobservations_board = Nobservations_board,
.observations_point = observations_point,
+ .observations_point_pool = observations_point_pool,
.Nobservations_point = Nobservations_point,
.observations_point_triangulated = observations_point_triangulated,
.Nobservations_point_triangulated = Nobservations_point_triangulated,
@@ -6635,10 +6635,13 @@ mrcal_optimize( // out
&dogleg_parameters,
&solver_context);
- if(norm2_error < 0)
+ if(norm2_error < 0) {
// libdogleg barfed. I quit out
- goto done;
+ if(solver_context != NULL)
+ dogleg_freeContext(&solver_context);
+ return stats;
+ }
#if 0
// Not using dogleg_markOutliers() (yet...)
@@ -6810,7 +6813,6 @@ mrcal_optimize( // out
if(x_final)
memcpy(x_final, solver_context->beforeStep->x, ctx.Nmeasurements*sizeof(double));
- done:
if(solver_context != NULL)
dogleg_freeContext(&solver_context);
@@ -6834,7 +6836,9 @@ bool mrcal_write_cameramodel_file(const char* filename,
{
MSG("Couldn't construct lensmodel string. Unconfigured string: '%s'",
mrcal_lensmodel_name_unconfigured(&cameramodel->lensmodel));
- goto done;
+ if(fp != NULL)
+ fclose(fp);
+ return result;
}
int Nparams = mrcal_lensmodel_num_params(&cameramodel->lensmodel);
@@ -6842,7 +6846,9 @@ bool mrcal_write_cameramodel_file(const char* filename,
{
MSG("Couldn't get valid Nparams from lensmodel string '%s'",
lensmodel_string);
- goto done;
+ if(fp != NULL)
+ fclose(fp);
+ return result;;
}
fprintf(fp, "{\n");
@@ -6865,7 +6871,6 @@ bool mrcal_write_cameramodel_file(const char* filename,
fprintf(fp,"}\n");
result = true;
- done:
if(fp != NULL)
fclose(fp);
return result;
diff --git a/poseutils.c b/poseutils.c
index 80d2b0d88dd5ad264bb579a7062b051d9972be74..59f7738477127829b87f490b5a2337c2378c2309 100644
--- a/poseutils.c
+++ b/poseutils.c
@@ -1079,215 +1079,6 @@ void mrcal_r_from_R_full( // output
}
}
-// LAPACK SVD function
-int dgesdd_(char* jobz,
- int* m,
- int* n,
- double* a,
- int* lda,
- double* s,
- double* u,
- int* ldu,
- double* vt,
- int* ldvt,
- double* work,
- int* lwork,
- int* iwork,
- int* info,
- int jobz_len);
-
-// This is functionally identical to mrcal.align_procrustes_vectors_R01(). It
-// should replace that function to provide a C implementation for mrcal users
-//
-// This solves:
-// https://en.wikipedia.org/wiki/Orthogonal_Procrustes_problem
-// See the mrcal sources for implementation details
-static
-bool _align_procrustes_vectors_R01(// out
- double* R01,
- // in
- const int N,
- // (N,3) arrays
- const double* p0,
- const double* p1,
- // (3,) array; may be NULL
- const double* pmean0,
- const double* pmean1,
-
- // (N,) array; may be NULL to use an even
- // weighting
- const double* weights)
-{
- double M[9] = {};
-
- double _pmean0[3] = {};
- double _pmean1[3] = {};
- if(pmean0 == NULL) pmean0 = _pmean0;
- if(pmean1 == NULL) pmean1 = _pmean1;
-
- if(weights == NULL)
- for(int i=0; i<N; i++)
- // I compute outer(v0,v1)
- for(int j=0; j<3; j++)
- for(int k=0; k<3; k++)
- M[j*3 + k] += (p0[i*3+j]-pmean0[j])*(p1[i*3+k]-pmean1[k]);
- else
- for(int i=0; i<N; i++)
- // I compute outer(v0,v1)
- for(int j=0; j<3; j++)
- for(int k=0; k<3; k++)
- M[j*3 + k] += (p0[i*3+j]-pmean0[j])*(p1[i*3+k]-pmean1[k])*weights[i];
-
-
- double U[9];
- double Vt[9];
- double S[3];
- double lwork_query;
- int iwork[3*8];
- int info;
-
- // lapack thinks about transposed matrices. So when I give it A, it sees At.
- // It computes A = U Vt -> At = V Ut. And the results it gives back to me
- // are transposed too. So I give it At. The "U" it gives me back is actually
- // Vt and the Vt is actually U
- dgesdd_("A",
- (int[]){3}, (int[]){3},
- M, (int[]){3},
- S,
- Vt,(int[]){3},
- U, (int[]){3},
- &lwork_query,
- (int[]){-1}, // query the optimal lwork
- iwork,
- &info,
- 1);
- if(info != 0)
- {
- // secret value to indicate that this is a fatal error. Needed for the
- // Python layer
- R01[0] = 1.;
- return false;
- }
-
- double work[(int)lwork_query];
-
- dgesdd_("A",
- (int[]){3}, (int[]){3},
- M, (int[]){3},
- S,
- Vt,(int[]){3},
- U, (int[]){3},
- work,
- (int[]){(int)lwork_query},
- iwork,
- &info,
- 1);
- if(info != 0)
- {
- // secret value to indicate that this is a fatal error. Needed for the
- // Python layer
- R01[0] = 1.;
- return false;
- }
-
- // I look at the second-lowest singular value. One 0 singular value is OK
- // (the other two can uniquely define my 3D basis). But two isn't OK: the
- // basis is no longer unique
- if(S[1] < 1e-12)
- {
- // Poorly-defined problem
- //
- // secret value to indicate that this is a potentially non-fatal error.
- // Needed for the Python layer
- R01[0] = 0.;
- return false;
- }
-
- memset(R01, 0, 9*sizeof(R01[0]));
- for(int i=0; i<3; i++)
- for(int j=0; j<3; j++)
- for(int k=0; k<3; k++)
- // inner( U[i,:], V[j,:]
- R01[i*3 + j] += U[i*3 + k]*Vt[j + k*3];
-
- // det(R01) is now +1 or -1. If it's -1, then this contains a mirror, and thus
- // is not a physical rotation. I compensate by negating the least-important
- // pair of singular vectors
- const double det_R =
- R01[0]*(R01[4]*R01[8]-R01[5]*R01[7]) -
- R01[1]*(R01[3]*R01[8]-R01[5]*R01[6]) +
- R01[2]*(R01[3]*R01[7]-R01[4]*R01[6]);
- if(det_R < 0)
- {
- memset(R01, 0, 9*sizeof(R01[0]));
-
- for(int i=0; i<3; i++)
- for(int j=0; j<3; j++)
- {
- int k;
- for(k=0; k<2; k++)
- R01[i*3 + j] += U[i*3 + k]*Vt[j + k*3];
- R01[i*3 + j] -= U[i*3 + k]*Vt[j + k*3];
- }
- }
-
- return true;
-}
-
-bool mrcal_align_procrustes_vectors_R01(// out
- double* R01,
- // in
- const int N,
- // (N,3) arrays
- const double* v0,
- const double* v1,
-
- // (N,) array; may be NULL to use an even
- // weighting
- const double* weights)
-{
- return _align_procrustes_vectors_R01(R01,N,v0,v1,NULL,NULL,weights);
-}
-
-bool mrcal_align_procrustes_points_Rt01(// out
- double* Rt01,
- // in
- const int N,
- // (N,3) arrays
- const double* p0,
- const double* p1,
-
- // (N,) array; may be NULL to use an even
- // weighting
- const double* weights)
-{
- double pmean0[3] = {};
- double pmean1[3] = {};
-
- for(int i=0; i<N; i++)
- for(int j=0; j<3; j++)
- {
- pmean0[j] += p0[i*3+j];
- pmean1[j] += p1[i*3+j];
- }
- for(int j=0; j<3; j++)
- {
- pmean0[j] /= (double)N;
- pmean1[j] /= (double)N;
- }
- if(!_align_procrustes_vectors_R01(Rt01,N,p0,p1,pmean0,pmean1,weights))
- return false;
-
- // t = pmean0 - R01 pmean1
- for(int i=0; i<3; i++)
- {
- Rt01[9 + i] = pmean0[i];
- for(int j=0; j<3; j++)
- Rt01[9 + i] -= Rt01[i*3 + j] * pmean1[j];
- }
- return true;
-}
-
// Compute a non-unique rotation to map a given vector to [0,0,1]
// See docstring for mrcal.R_aligned_to_vector() for details
void mrcal_R_aligned_to_vector(// out

37
upstream_utils/mrcal_patches/0007-Place-all-C-headers-in-an-extern-C-block.patch Normal file
View File

@@ -0,0 +1,37 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Gold856 <117957790+Gold856@users.noreply.github.com>
Date: Wed, 4 Dec 2024 00:58:21 -0500
Subject: [PATCH 7/8] Place all C headers in an extern "C" block
---
mrcal.cpp | 5 ++++-
1 file changed, 4 insertions(+), 1 deletion(-)
diff --git a/mrcal.cpp b/mrcal.cpp
index 1560cd106ff5bcfe49476f785aa8108119318b3a..d4a2415a5730a48671b2716960bc343cdbd01295 100644
--- a/mrcal.cpp
+++ b/mrcal.cpp
@@ -14,19 +14,22 @@
#include <stdlib.h>
#include <inttypes.h>
+extern "C" {
#include <dogleg.h>
+}
#include <assert.h>
#include <stdbool.h>
#include <math.h>
#include <string.h>
+extern "C" {
#include "mrcal.h"
#include "minimath/minimath.h"
#include "cahvore.h"
#include "minimath/minimath-extra.h"
#include "util.h"
#include "scales.h"
-
+}
#define MSG_IF_VERBOSE(...) do { if(verbose) MSG( __VA_ARGS__ ); } while(0)
#define restrict __restrict

70
upstream_utils/mrcal_patches/0008-Fix-UB-in-mrcal.patch Normal file
View File

@@ -0,0 +1,70 @@
From 0000000000000000000000000000000000000000 Mon Sep 17 00:00:00 2001
From: Matt <matthew.morley.ca@gmail.com>
Date: Thu, 5 Dec 2024 00:10:15 -0500
Subject: [PATCH 8/8] Fix UB in mrcal
---
mrcal.cpp | 27 ++++++++++++++++-----------
1 file changed, 16 insertions(+), 11 deletions(-)
diff --git a/mrcal.cpp b/mrcal.cpp
index d4a2415a5730a48671b2716960bc343cdbd01295..0b7263f58164bfe9f42447741a08366262195f7c 100644
--- a/mrcal.cpp
+++ b/mrcal.cpp
@@ -4582,7 +4582,6 @@ void optimizer_callback(// input state
// Construct the FULL intrinsics vector, based on either the
// optimization vector or the inputs, depending on what we're optimizing
double* intrinsics_here = &intrinsics_all[icam_intrinsics][0];
- double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore];
int i_var_intrinsics =
mrcal_state_index_intrinsics(icam_intrinsics,
@@ -4604,15 +4603,20 @@ void optimizer_callback(// input state
&ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics],
Ncore*sizeof(double) );
}
- if( ctx->problem_selections.do_optimize_intrinsics_distortions )
- {
- for(int i = 0; i<ctx->Nintrinsics-Ncore; i++)
- distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION;
+ int nDistortion = ctx->Nintrinsics-Ncore;
+ if (nDistortion) {
+ double* distortions_here = &intrinsics_all[icam_intrinsics][Ncore];
+ if( ctx->problem_selections.do_optimize_intrinsics_distortions )
+ {
+ for(int i = 0; i<ctx->Nintrinsics-Ncore; i++)
+ distortions_here[i] = packed_state[i_var_intrinsics++] * SCALE_DISTORTION;
+ }
+ else {
+ memcpy( distortions_here,
+ &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore],
+ (ctx->Nintrinsics-Ncore)*sizeof(double) );
+ }
}
- else
- memcpy( distortions_here,
- &ctx->intrinsics[ctx->Nintrinsics*icam_intrinsics + Ncore],
- (ctx->Nintrinsics-Ncore)*sizeof(double) );
}
for(int icam_extrinsics=0;
icam_extrinsics<ctx->Ncameras_extrinsics;
@@ -4699,6 +4703,7 @@ void optimizer_callback(// input state
int splined_intrinsics_grad_irun = 0;
+ bool camera_at_identity = icam_extrinsics < 0;
project(q_hypothesis.data(),
ctx->problem_selections.do_optimize_intrinsics_core || ctx->problem_selections.do_optimize_intrinsics_distortions ?
@@ -4720,9 +4725,9 @@ void optimizer_callback(// input state
// input
intrinsics_all[icam_intrinsics].data(),
- &camera_rt[icam_extrinsics], &frame_rt,
+ camera_at_identity ? NULL : &camera_rt[icam_extrinsics], &frame_rt,
ctx->calobject_warp == NULL ? NULL : &calobject_warp_local,
- icam_extrinsics < 0,
+ camera_at_identity,
&ctx->lensmodel, &ctx->precomputed,
ctx->calibration_object_spacing,
ctx->calibration_object_width_n,

36
wpical/.styleguide Normal file
View File

@@ -0,0 +1,36 @@
cppHeaderFileInclude {
\.h$
\.inc$
\.inl$
}
cppSrcFileInclude {
\.cpp$
}
generatedFileExclude {
src/main/native/resources/
src/main/native/thirdparty/
src/main/native/win/wpical.ico
src/main/native/mac/wpical.icns
}
repoRootNameOverride {
wpical
}
includeOtherLibs {
^GLFW
^ceres/
^fmt/
^frc/
^gtest/
^imgui
^implot\.h$
^mrcal_wrapper\.h$
^opencv2\.h$
^portable-file-dialogs\.h$
^tagpose\.h$
^wpi/
^wpigui
}

129
wpical/CMakeLists.txt Normal file
View File

@@ -0,0 +1,129 @@
project(wpical LANGUAGES C CXX Fortran)
include(CompileWarnings)
include(GenResources)
include(LinkMacOSGUI)
include(AddTest)
configure_file(src/main/generate/WPILibVersion.cpp.in WPILibVersion.cpp)
generate_resources(src/main/native/resources generated/main/cpp WPIcal wpical wpical_resources_src)
file(GLOB wpical_src src/main/native/cpp/*.cpp ${CMAKE_CURRENT_BINARY_DIR}/WPILibVersion.cpp)
file(
GLOB_RECURSE wpical_thirdparty_src
src/main/native/thirdparty/libdogleg/src/*.cpp
src/main/native/thirdparty/mrcal/src/*.c
src/main/native/thirdparty/mrcal/src/*.cpp
src/main/native/thirdparty/mrcal_java/src/*.cpp
)
if(WIN32)
set(wpical_rc src/main/native/win/wpical.rc)
elseif(APPLE)
set(MACOSX_BUNDLE_ICON_FILE wpical.icns)
set(APP_ICON_MACOSX src/main/native/mac/wpical.icns)
set_source_files_properties(${APP_ICON_MACOSX} PROPERTIES MACOSX_PACKAGE_LOCATION "Resources")
endif()
add_executable(
wpical
${wpical_src}
${wpical_thirdparty_src}
${wpical_resources_src}
${wpical_rc}
${APP_ICON_MACOSX}
)
wpilib_link_macos_gui(wpical)
wpilib_target_warnings(wpical)
target_include_directories(
wpical
PRIVATE
src/main/native/include
src/main/native/thirdparty/libdogleg/include
src/main/native/thirdparty/mrcal/include
src/main/native/thirdparty/mrcal_java/include
)
if(MSVC)
set(compile_flags
/wd4047
/wd4098
/wd4267
/wd4068
/wd4101
/wd4200
/wd4576
/wd4715
)
elseif(CMAKE_CXX_COMPILER_ID MATCHES "Clang")
set(compile_flags
-Wno-format-nonliteral
-Wno-unused-variable
-Wno-missing-field-initializers
-Wno-gnu-anonymous-struct
-Wno-c99-extensions
-Wno-zero-length-array
-Wno-nested-anon-types
-Wno-sign-compare
-Wno-unused-function
-Wno-missing-braces
-Wno-null-conversion
-Wno-unused-but-set-variable
)
else()
set(compile_flags
-Wno-format-nonliteral
-Wno-unused-variable
-Wno-unused-function
-Wno-sign-compare
-Wno-missing-field-initializers
)
endif()
target_compile_options(wpical PRIVATE ${compile_flags})
find_package(OpenCV REQUIRED)
find_package(Ceres CONFIG REQUIRED)
target_link_libraries(
wpical
apriltag
${OpenCV_LIBS}
wpigui
wpiutil
Ceres::ceres
)
if(WIN32)
set_target_properties(wpical PROPERTIES WIN32_EXECUTABLE YES)
elseif(APPLE)
set_target_properties(wpical PROPERTIES MACOSX_BUNDLE YES OUTPUT_NAME "wpical")
endif()
if(WITH_TESTS)
wpilib_add_test(wpical src/test/native/cpp)
wpilib_link_macos_gui(wpical_test)
target_sources(wpical_test PRIVATE ${wpical_src} ${wpical_thirdparty_src})
target_compile_definitions(wpical_test PRIVATE RUNNING_WPICAL_TESTS)
if(MSVC)
target_compile_options(wpical_test PRIVATE /utf-8)
endif()
target_compile_options(wpical_test PRIVATE ${compile_flags})
target_include_directories(
wpical_test
PRIVATE
src/main/native/include
src/main/native/thirdparty/libdogleg/include
src/main/native/thirdparty/mrcal/include
src/main/native/thirdparty/mrcal_java/include
)
target_link_libraries(
wpical_test
googletest
apriltag
${OpenCV_LIBS}
wpigui
wpiutil
Ceres::ceres
)
endif()

32
wpical/Info.plist Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE plist PUBLIC "-//Apple//DTD PLIST 1.0//EN" "http://www.apple.com/DTDs/PropertyList-1.0.dtd">
<plist version="1.0">
<dict>
<key>CFBundleName</key>
<string>WPIcal</string>
<key>CFBundleExecutable</key>
<string>wpical</string>
<key>CFBundleDisplayName</key>
<string>WPIcal</string>
<key>CFBundleIdentifier</key>
<string>edu.wpi.first.tools.WPIcal</string>
<key>CFBundleIconFile</key>
<string>ov.icns</string>
<key>CFBundlePackageType</key>
<string>APPL</string>
<key>CFBundleSupportedPlatforms</key>
<array>
<string>MacOSX</string>
</array>
<key>CFBundleInfoDictionaryVersion</key>
<string>6.0</string>
<key>CFBundleShortVersionString</key>
<string>2021</string>
<key>CFBundleVersion</key>
<string>2021</string>
<key>LSMinimumSystemVersion</key>
<string>10.11</string>
<key>NSHighResolutionCapable</key>
<true/>
</dict>
</plist>

249
wpical/build.gradle Normal file
View File

@@ -0,0 +1,249 @@
import org.gradle.internal.os.OperatingSystem
if (project.hasProperty('onlylinuxathena') || project.hasProperty('onlylinuxarm32') || project.hasProperty('onlylinuxarm64')) {
return;
}
description = "Field Calibration Tool"
apply plugin: 'c'
apply plugin: 'cpp'
apply plugin: 'visual-studio'
apply plugin: 'google-test-test-suite'
apply plugin: 'edu.wpi.first.NativeUtils'
if (OperatingSystem.current().isWindows()) {
apply plugin: 'windows-resources'
}
ext {
nativeName = 'wpical'
useCpp = true
sharedCvConfigs = [
wpicalTest: []]
staticCvConfigs = []
}
apply from: "${rootDir}/shared/resources.gradle"
apply from: "${rootDir}/shared/config.gradle"
def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in")
def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp")
apply from: "${rootDir}/shared/ceres.gradle"
apply from: "${rootDir}/shared/opencv.gradle"
task generateCppVersion() {
description = 'Generates the wpilib version class'
group = 'WPILib'
outputs.file wpilibVersionFileOutput
inputs.file wpilibVersionFileInput
if (wpilibVersioning.releaseMode) {
outputs.upToDateWhen { false }
}
// We follow a simple set of checks to determine whether we should generate a new version file:
// 1. If the release type is not development, we generate a new version file
// 2. If there is no generated version number, we generate a new version file
// 3. If there is a generated build number, and the release type is development, then we will
// only generate if the publish task is run.
doLast {
def version = wpilibVersioning.version.get()
println "Writing version ${version} to $wpilibVersionFileOutput"
if (wpilibVersionFileOutput.exists()) {
wpilibVersionFileOutput.delete()
}
def read = wpilibVersionFileInput.text.replace('${wpilib_version}', version)
wpilibVersionFileOutput.write(read)
}
}
gradle.taskGraph.addTaskExecutionGraphListener { graph ->
def willPublish = graph.hasTask(publish)
if (willPublish) {
generateCppVersion.outputs.upToDateWhen { false }
}
}
def generateTask = createGenerateResourcesTask('main', 'WPIcal', 'wpical', project)
project(':').libraryBuild.dependsOn build
tasks.withType(CppCompile) {
dependsOn generateTask
dependsOn generateCppVersion
}
// Suppress warnings
nativeUtils.platformConfigs.each {
if (it.name.contains('windows')) {
it.cCompiler.args.add("/wd4047")
it.cCompiler.args.add("/wd4098")
it.cCompiler.args.add("/wd4267")
it.cppCompiler.args.add("/wd4068")
it.cppCompiler.args.add("/wd4101")
it.cppCompiler.args.add("/wd4200")
it.cppCompiler.args.add("/wd4576")
it.cppCompiler.args.add("/wd4715")
} else if (it.name.contains('osx')) {
it.cCompiler.args.add("-Wno-format-nonliteral")
it.cCompiler.args.remove("-pedantic")
it.cCompiler.args.add("-Wno-unused-variable")
it.cCompiler.args.add("-Wno-unused-function")
it.cCompiler.args.add("-Wno-sign-compare")
it.cppCompiler.args.add("-Wno-missing-field-initializers")
it.cppCompiler.args.remove("-pedantic")
it.cppCompiler.args.add("-Wno-unused-variable")
it.cppCompiler.args.add("-Wno-unused-function")
it.cppCompiler.args.add("-Wno-sign-compare")
it.cppCompiler.args.remove("-permissive")
it.cppCompiler.args.add("-fpermissive")
it.cppCompiler.args.add("-Wno-missing-braces")
it.cppCompiler.args.add("-Wno-null-conversion")
it.cppCompiler.args.add("-Wno-unused-but-set-variable")
} else {
it.cCompiler.args.add("-Wno-format-nonliteral")
it.cCompiler.args.remove("-pedantic")
it.cCompiler.args.add("-Wno-unused-variable")
it.cCompiler.args.add("-Wno-unused-function")
it.cCompiler.args.add("-Wno-sign-compare")
it.cppCompiler.args.add("-Wno-missing-field-initializers")
it.cppCompiler.args.remove("-pedantic")
it.cppCompiler.args.add("-Wno-unused-variable")
it.cppCompiler.args.add("-Wno-unused-function")
it.cppCompiler.args.add("-Wno-sign-compare")
it.cppCompiler.args.add("-Wno-deprecated-declarations")
it.cppCompiler.args.add("-Wno-deprecated-enum-enum-conversion")
it.cppCompiler.args.remove("-permissive")
it.cppCompiler.args.add("-fpermissive")
}
}
def testResources = "\"$rootDir/wpical/src/main/native/resources\"".replace("\\", "/")
model {
components {
"${nativeName}"(NativeExecutableSpec) {
baseName = 'wpical'
sources {
cpp {
source {
srcDirs 'src/main/native/cpp', 'src/main/native/thirdparty/libdogleg/src', 'src/main/native/thirdparty/mrcal/src', 'src/main/native/thirdparty/mrcal_java/src', "$buildDir/generated/main/cpp"
include '**/*.cpp'
}
exportedHeaders {
srcDirs 'src/main/native/include', 'src/main/native/thirdparty/libdogleg/include', 'src/main/native/thirdparty/mrcal/generated', 'src/main/native/thirdparty/mrcal/include', 'src/main/native/thirdparty/mrcal/include/minimath', 'src/main/native/thirdparty/mrcal_java/include'
}
}
c {
source {
srcDirs 'src/main/native/thirdparty/mrcal/src'
include '**/*.c'
}
exportedHeaders {
srcDirs 'src/main/native/include', 'src/main/native/thirdparty/libdogleg/include', 'src/main/native/thirdparty/mrcal/generated', 'src/main/native/thirdparty/mrcal/include', 'src/main/native/thirdparty/mrcal/include/minimath'
}
}
if (OperatingSystem.current().isWindows()) {
rc.source {
srcDirs 'src/main/native/win'
include '*.rc'
}
}
}
binaries.all {
if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name.startsWith("linuxarm")) {
it.buildable = false
return
}
lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
lib project: ':wpigui', library: 'wpigui', linkage: 'static'
lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static'
lib project: ':apriltag', library: 'apriltag', linkage: 'static'
nativeUtils.useRequiredLibrary(it, 'ceres')
nativeUtils.useRequiredLibrary(it, 'opencv_static')
it.cppCompiler.define 'GLOG_USE_GLOG_EXPORT'
if (it.targetPlatform.operatingSystem.isWindows()) {
it.cppCompiler.define('GLOG_DEPRECATED', '__declspec(deprecated)')
} else {
it.cppCompiler.define('GLOG_DEPRECATED', '[[deprecated]]')
}
if (it.targetPlatform.operatingSystem.isWindows()) {
// Comdlg32.lib is needed for GetSaveFileNameA
// dbghelp.lib is needed for glog functions
it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib' << 'Comdlg32.lib' << 'dbghelp.lib'
} else if (it.targetPlatform.operatingSystem.isMacOsX()) {
it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' << '-framework' << 'Accelerate'
} else {
it.linker.args << '-lX11' << "-lgfortran"
if (it.targetPlatform.name.startsWith('linuxarm')) {
it.linker.args << '-lGL'
}
}
}
}
}
testSuites {
"${nativeName}Test"(GoogleTestTestSuiteSpec) {
for(NativeComponentSpec c : $.components) {
if (c.name == nativeName) {
testing c
break
}
}
sources {
cpp {
source {
srcDirs 'src/test/native/cpp'
include '**/*.cpp'
}
exportedHeaders {
srcDirs 'src/test/native/include', 'src/main/native/cpp'
}
}
}
binaries.all {
if (it.targetPlatform.name == nativeUtils.wpi.platforms.roborio || it.targetPlatform.name.startsWith("linuxarm")) {
it.buildable = false
return
}
lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
lib project: ':wpigui', library: 'wpigui', linkage: 'static'
lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static'
lib project: ':apriltag', library: 'apriltag', linkage: 'static'
nativeUtils.useRequiredLibrary(it, 'ceres')
nativeUtils.useRequiredLibrary(it, 'opencv_static')
it.cppCompiler.define('PROJECT_ROOT_PATH', testResources)
it.cppCompiler.define 'GLOG_USE_GLOG_EXPORT'
if (it.targetPlatform.name == nativeUtils.wpi.platforms.windowsarm64) {
// https://developercommunity.visualstudio.com/t/-imp-std-init-once-complete-unresolved-external-sy/1684365#T-N10041864
it.linker.args << '/alternatename:__imp___std_init_once_complete=__imp_InitOnceComplete'
it.linker.args << '/alternatename:__imp___std_init_once_begin_initialize=__imp_InitOnceBeginInitialize'
}
if (it.targetPlatform.operatingSystem.isWindows()) {
it.cppCompiler.define('GLOG_DEPRECATED', '__declspec(deprecated)')
} else {
it.cppCompiler.define('GLOG_DEPRECATED', '[[deprecated]]')
}
lib project: ':thirdparty:googletest', library: 'googletest', linkage: 'static'
it.cppCompiler.define("RUNNING_WPICAL_TESTS")
if (it.targetPlatform.operatingSystem.isWindows()) {
it.linker.args << 'Gdi32.lib' << 'Shell32.lib' << 'd3d11.lib' << 'd3dcompiler.lib'
// Comdlg32.lib is needed for GetSaveFileNameA
// dbghelp.lib is needed for glog functions
// advapi32.lib is needed for __imp_RegDeleteKeyA and other __imp_Reg functions for icvLoadWindowPos and the save variant
it.linker.args << 'advapi32.lib' << 'Comdlg32.lib' << 'dbghelp.lib'
} else if (it.targetPlatform.operatingSystem.isMacOsX()) {
it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' << '-framework' << 'Accelerate'
} else {
it.linker.args << '-lX11' << '-lgfortran'
if (it.targetPlatform.name.startsWith('linuxarm')) {
it.linker.args << '-lGL'
}
}
}
}
}
}
apply from: 'publish.gradle'

32
wpical/generate_mrcal.py Executable file
View File

@@ -0,0 +1,32 @@
#!/usr/bin/env python3
import argparse
import subprocess
import sys
from pathlib import Path
def main(argv):
script_path = Path(__file__).resolve()
dirname = script_path.parent
parser = argparse.ArgumentParser()
parser.add_argument(
"--output_directory",
help="Optional. If set, will output the generated files to this directory, otherwise it will use a path relative to the script",
default=dirname / "src/main/native/thirdparty/mrcal/generated/",
type=Path,
)
args = parser.parse_args(argv)
args.output_directory.mkdir(parents=True, exist_ok=True)
result = subprocess.run(
f"{dirname}/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl",
capture_output=True,
)
(args.output_directory / "minimath_generated.h").write_text(
str(result.stdout, encoding="UTF8")
)
if __name__ == "__main__":
main(sys.argv[1:])

124
wpical/publish.gradle Normal file
View File

@@ -0,0 +1,124 @@
apply plugin: 'maven-publish'
def baseArtifactId = 'wpical'
def artifactGroupId = 'edu.wpi.first.tools'
def zipBaseName = '_GROUP_edu_wpi_first_tools_ID_wpical_CLS'
def outputsFolder = file("$project.buildDir/outputs")
model {
tasks {
// Create the run task.
$.components.wpical.binaries.each { bin ->
if (bin.buildable && bin.name.toLowerCase().contains("debug") && nativeUtils.isNativeDesktopPlatform(bin.targetPlatform)) {
Task run = project.tasks.create("run", Exec) {
commandLine bin.tasks.install.runScriptFile.get().asFile.toString()
}
run.dependsOn bin.tasks.install
}
}
}
publishing {
def wpicalTaskList = []
$.components.each { component ->
component.binaries.each { binary ->
if (binary in NativeExecutableBinarySpec && binary.component.name.contains("wpical")) {
if (binary.buildable && (binary.name.contains('Release') || binary.name.contains('release'))) {
// We are now in the binary that we want.
// This is the default application path for the ZIP task.
def applicationPath = binary.executable.file
def icon = file("$project.projectDir/src/main/native/mac/wpical.icns")
// Create the ZIP.
def task = project.tasks.create("copywpicalExecutable" + binary.targetPlatform.operatingSystem.name + binary.targetPlatform.architecture.name, Zip) {
description("Copies the wpical executable to the outputs directory.")
destinationDirectory = outputsFolder
archiveBaseName = zipBaseName
duplicatesStrategy = 'exclude'
archiveClassifier = nativeUtils.getPublishClassifier(binary)
from(licenseFile) {
into '/'
}
if (binary.targetPlatform.operatingSystem.isWindows()) {
def exePath = binary.executable.file.absolutePath
exePath = exePath.substring(0, exePath.length() - 4)
def pdbPath = new File(exePath + '.pdb')
from(pdbPath)
}
into(nativeUtils.getPlatformPath(binary))
}
if (binary.targetPlatform.operatingSystem.isMacOsX()) {
// Create the macOS bundle.
def bundleTask = project.tasks.create("bundlewpicalOsxApp" + binary.targetPlatform.architecture.name, Copy) {
description("Creates a macOS application bundle for wpical")
from(file("$project.projectDir/Info.plist"))
into(file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/wpical.app/Contents"))
into("MacOS") {
with copySpec {
from binary.executable.file
}
}
into("Resources") {
with copySpec {
from icon
}
}
inputs.property "HasDeveloperId", project.hasProperty("developerID")
doLast {
if (project.hasProperty("developerID")) {
// Get path to binary.
exec {
workingDir rootDir
def args = [
"sh",
"-c",
"codesign --force --strict --deep " +
"--timestamp --options=runtime " +
"--verbose -s ${project.findProperty("developerID")} " +
"$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name/wpical.app/"
]
commandLine args
}
}
}
}
// Reset the application path if we are creating a bundle.
applicationPath = file("$project.buildDir/outputs/bundles/$binary.targetPlatform.architecture.name")
task.from(applicationPath)
project.build.dependsOn bundleTask
bundleTask.dependsOn binary.tasks.link
task.dependsOn(bundleTask)
} else {
task.from(applicationPath)
}
task.dependsOn binary.tasks.link
wpicalTaskList.add(task)
project.build.dependsOn task
project.artifacts { task }
addTaskToCopyAllOutputs(task)
}
}
}
}
publications {
wpical(MavenPublication) {
wpicalTaskList.each { artifact it }
artifactId = baseArtifactId
groupId = artifactGroupId
version wpilibVersioning.version.get()
}
}
}
}

7
wpical/src/main/generate/WPILibVersion.cpp.in Normal file
View File

@@ -0,0 +1,7 @@
/**
* Autogenerated file! Do not manually edit this file. This version is regenerated
* any time the publish task is run, or when this file is deleted.
*/
const char* GetWPILibVersion() {
return "${wpilib_version}";
}

591
wpical/src/main/native/cpp/WPIcal.cpp Normal file
View File

@@ -0,0 +1,591 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <cameracalibration.h>
#include <fieldcalibration.h>
#include <fieldmap.h>
#include <fmap.h>
#include <fstream>
#include <iostream>
#include <memory>
#include <numbers>
#include <string>
#include <vector>
#include <GLFW/glfw3.h>
#include <imgui.h>
#include <portable-file-dialogs.h>
#include <tagpose.h>
#include <wpi/json.h>
#include <wpigui.h>
namespace gui = wpi::gui;
const char* GetWPILibVersion();
#ifdef __linux__
const bool showDebug = false;
#else
const bool showDebug = true;
#endif
namespace wpical {
std::string_view GetResource_wpical_16_png();
std::string_view GetResource_wpical_32_png();
std::string_view GetResource_wpical_48_png();
std::string_view GetResource_wpical_64_png();
std::string_view GetResource_wpical_128_png();
std::string_view GetResource_wpical_256_png();
std::string_view GetResource_wpical_512_png();
} // namespace wpical
void drawCheck() {
ImGui::SameLine();
ImDrawList* draw_list = ImGui::GetWindowDrawList();
ImVec2 pos = ImGui::GetCursorScreenPos();
float size = ImGui::GetFontSize();
ImVec2 p1 = ImVec2(pos.x + size * 0.25f, pos.y + size * 0.5f);
ImVec2 p2 = ImVec2(pos.x + size * 0.45f, pos.y + size * 0.7f);
ImVec2 p3 = ImVec2(pos.x + size * 0.75f, pos.y + size * 0.3f);
float thickness = 3.0f;
draw_list->AddLine(p1, p2, IM_COL32(0, 255, 0, 255), thickness);
draw_list->AddLine(p2, p3, IM_COL32(0, 255, 0, 255), thickness);
ImGui::NewLine();
}
static void DisplayGui() {
ImGui::GetStyle().WindowRounding = 0;
// fill entire OS window with this window
ImGui::SetNextWindowPos(ImVec2(0, 0));
int width, height;
glfwGetWindowSize(gui::GetSystemWindow(), &width, &height);
ImGui::SetNextWindowSize(
ImVec2(static_cast<float>(width), static_cast<float>(height)));
ImGui::Begin("Entries", nullptr,
ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_MenuBar |
ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove |
ImGuiWindowFlags_NoCollapse);
// main menu
ImGui::BeginMenuBar();
gui::EmitViewMenu();
if (ImGui::BeginMenu("View")) {
ImGui::EndMenu();
}
ImGui::EndMenuBar();
static std::unique_ptr<pfd::open_file> camera_intrinsics_selector;
static std::string selected_camera_intrinsics;
static std::unique_ptr<pfd::open_file> field_map_selector;
static std::string selected_field_map;
static std::unique_ptr<pfd::select_folder>
field_calibration_directory_selector;
static std::string selected_field_calibration_directory;
static std::unique_ptr<pfd::select_folder> download_directory_selector;
static std::string selected_download_directory;
static std::string calibration_json_path;
cameracalibration::CameraModel cameraModel = {
.intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
.distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
.avg_reprojection_error = 0.0};
static bool mrcal = true;
static double squareWidth = 0.709;
static double markerWidth = 0.551;
static int boardWidth = 12;
static int boardHeight = 8;
static double imagerWidth = 1920;
static double imagerHeight = 1080;
static int pinnedTag = 1;
static int focusedTag = 1;
static int referenceTag = 1;
static Fieldmap currentCalibrationMap;
static Fieldmap currentReferenceMap;
// camera matrix selector button
if (ImGui::Button("Upload Camera Intrinsics")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Intrinsics JSON", "",
std::vector<std::string>{"JSON", "*.json"}, pfd::opt::none);
}
ImGui::SameLine();
ImGui::Text("Or");
ImGui::SameLine();
// camera calibration button
if (ImGui::Button("Calibrate Camera")) {
selected_camera_intrinsics.clear();
ImGui::OpenPopup("Camera Calibration");
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
if (!selected_camera_intrinsics.empty()) {
drawCheck();
}
// field json selector button
if (ImGui::Button("Select Field Map JSON")) {
field_map_selector = std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none);
}
if (field_map_selector) {
auto selectedFiles = field_map_selector->result();
if (!selectedFiles.empty()) {
selected_field_map = selectedFiles[0];
}
field_map_selector.reset();
}
if (!selected_field_map.empty()) {
drawCheck();
}
// field calibration directory selector button
if (ImGui::Button("Select Field Calibration Folder")) {
field_calibration_directory_selector = std::make_unique<pfd::select_folder>(
"Select Field Calibration Folder", "");
}
if (field_calibration_directory_selector) {
auto selectedFiles = field_calibration_directory_selector->result();
if (!selectedFiles.empty()) {
selected_field_calibration_directory = selectedFiles;
}
field_calibration_directory_selector.reset();
}
if (!selected_field_calibration_directory.empty()) {
drawCheck();
}
// pinned tag text field
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Pinned Tag", &pinnedTag);
if (pinnedTag < 1) {
pinnedTag = 1;
} else if (pinnedTag > 16) {
pinnedTag = 16;
}
// calibrate button
if (ImGui::Button("Calibrate!!!")) {
if (!selected_field_calibration_directory.empty() &&
!selected_camera_intrinsics.empty() && !selected_field_map.empty() &&
pinnedTag > 0 && pinnedTag <= 16) {
download_directory_selector =
std::make_unique<pfd::select_folder>("Select Download Folder", "");
if (download_directory_selector) {
auto selectedFiles = download_directory_selector->result();
if (!selectedFiles.empty()) {
selected_download_directory = selectedFiles;
}
download_directory_selector.reset();
}
calibration_json_path = selected_download_directory + "/output.json";
int calibrationOutput = fieldcalibration::calibrate(
selected_field_calibration_directory.c_str(), calibration_json_path,
selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
showDebug);
if (calibrationOutput == 1) {
ImGui::OpenPopup("Fmap Conversion failed");
} else if (calibrationOutput == 0) {
std::ifstream caljsonpath(calibration_json_path);
try {
wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath));
std::ofstream out(selected_download_directory + "/output.fmap");
out << fmap.dump(4);
out.close();
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Visualize Calibration");
} catch (...) {
ImGui::OpenPopup("Field Calibration Error");
}
}
}
}
if (ImGui::Button("Visualize")) {
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Visualize Calibration");
}
if (selected_field_calibration_directory.empty() ||
selected_camera_intrinsics.empty() || selected_field_map.empty()) {
ImGui::TextWrapped(
"Some inputs are empty! please enter your camera calibration video, "
"field map, and field calibration directory");
} else if (!(pinnedTag > 0 && pinnedTag <= 16)) {
ImGui::TextWrapped("Make sure the pinned tag is a valid april tag (1-16)");
} else {
ImGui::TextWrapped("Calibration Ready");
}
// error popup window
if (ImGui::BeginPopupModal("Field Calibration Error", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
ImGui::TextWrapped(
"Field Calibration Failed - please try again, ensuring that:");
ImGui::TextWrapped(
"- You have selected the correct camera intrinsics JSON or camera "
"calibration video");
ImGui::TextWrapped("- You have selected the correct ideal field map JSON");
ImGui::TextWrapped(
"- You have selected the correct field calibration video directory");
ImGui::TextWrapped(
"- Your field calibration video directory contains only field "
"calibration videos and no other files");
ImGui::TextWrapped("- Your pinned tag is a valid FRC Apriltag");
ImGui::Separator();
if (ImGui::Button("OK", ImVec2(120, 0))) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
if (ImGui::BeginPopupModal("Fmap Conversion failed", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
ImGui::TextWrapped(
"Fmap conversion failed - you can still use the calibration output on "
"Limelight platforms by converting the .json output to .fmap using the "
"Limelight map builder tool");
ImGui::TextWrapped("https://tools.limelightvision.io/map-builder");
ImGui::Separator();
if (ImGui::Button("OK", ImVec2(120, 0))) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
// successful field calibration popup window
if (ImGui::BeginPopupModal("Success", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
ImGui::TextWrapped("Success, output JSON generated in selected directory");
ImGui::Separator();
if (ImGui::Button("OK", ImVec2(120, 0))) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
// camera calibration popup window
if (ImGui::BeginPopupModal("Camera Calibration", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
// Camera Calibration Error calibration popup window
if (ImGui::BeginPopupModal("Camera Calibration Error", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
ImGui::TextWrapped(
"Camera calibration failed. Please make sure you have uploaded the "
"correct video file");
ImGui::Separator();
if (ImGui::Button("OK", ImVec2(120, 0))) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
if (ImGui::Button("MRcal")) {
mrcal = true;
}
ImGui::SameLine();
ImGui::Text("or");
ImGui::SameLine();
if (ImGui::Button("OpenCV")) {
mrcal = false;
}
if (mrcal) {
if (ImGui::Button("Select Camera Calibration Video")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Calibration Video", "",
std::vector<std::string>{"Video Files",
"*.mp4 *.mov *.m4v *.mkv *.avi"},
pfd::opt::none);
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Square Width (in)", &squareWidth);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Marker Width (in)", &markerWidth);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Board Width (squares)", &boardWidth);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Board Height (squares)", &boardHeight);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Image Width (pixels)", &imagerWidth);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Image Height (pixels)", &imagerHeight);
ImGui::Separator();
if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) {
std::cout << "calibration button pressed" << std::endl;
int ret = cameracalibration::calibrate(
selected_camera_intrinsics.c_str(), cameraModel, markerWidth,
boardWidth, boardHeight, imagerWidth, imagerHeight, showDebug);
if (ret == 0) {
size_t lastSeparatorPos =
selected_camera_intrinsics.find_last_of("/\\");
std::string output_file_path;
if (lastSeparatorPos != std::string::npos) {
output_file_path =
selected_camera_intrinsics.substr(0, lastSeparatorPos)
.append("/cameracalibration.json");
}
selected_camera_intrinsics = output_file_path;
cameracalibration::dumpJson(cameraModel, output_file_path);
ImGui::CloseCurrentPopup();
} else if (ret == 1) {
std::cout << "calibration failed and popup ready" << std::endl;
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Camera Calibration Error");
} else {
ImGui::CloseCurrentPopup();
}
}
} else {
if (ImGui::Button("Select Camera Calibration Video")) {
camera_intrinsics_selector = std::make_unique<pfd::open_file>(
"Select Camera Calibration Video", "",
std::vector<std::string>{"Video Files",
"*.mp4 *.mov *.m4v *.mkv *.avi"},
pfd::opt::none);
}
if (camera_intrinsics_selector) {
auto selectedFiles = camera_intrinsics_selector->result();
if (!selectedFiles.empty()) {
selected_camera_intrinsics = selectedFiles[0];
}
camera_intrinsics_selector.reset();
}
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Square Width (in)", &squareWidth);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputDouble("Marker Width (in)", &markerWidth);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Board Width (squares)", &boardWidth);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Board Height (squares)", &boardHeight);
ImGui::Separator();
if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) {
std::cout << "calibration button pressed" << std::endl;
int ret = cameracalibration::calibrate(
selected_camera_intrinsics.c_str(), cameraModel, squareWidth,
markerWidth, boardWidth, boardHeight, showDebug);
if (ret == 0) {
size_t lastSeparatorPos =
selected_camera_intrinsics.find_last_of("/\\");
std::string output_file_path;
if (lastSeparatorPos != std::string::npos) {
output_file_path =
selected_camera_intrinsics.substr(0, lastSeparatorPos)
.append("/cameracalibration.json");
}
selected_camera_intrinsics = output_file_path;
cameracalibration::dumpJson(cameraModel, output_file_path);
ImGui::CloseCurrentPopup();
} else if (ret == 1) {
std::cout << "calibration failed and popup ready" << std::endl;
ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
ImGui::OpenPopup("Camera Calibration Error");
} else {
ImGui::CloseCurrentPopup();
}
}
}
ImGui::SameLine();
if (ImGui::Button("Close")) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
// visualize calibration popup
if (ImGui::BeginPopupModal("Visualize Calibration", NULL,
ImGuiWindowFlags_AlwaysAutoResize)) {
if (ImGui::Button("Load Calibrated Field")) {
calibration_json_path =
std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
->result()[0];
}
if (!calibration_json_path.empty()) {
ImGui::SameLine();
drawCheck();
}
if (ImGui::Button("Load Reference Field")) {
selected_field_map =
std::make_unique<pfd::open_file>(
"Select Json File", "",
std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
->result()[0];
}
if (!selected_field_map.empty()) {
ImGui::SameLine();
drawCheck();
}
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Focused Tag", &focusedTag);
ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
ImGui::InputInt("Reference Tag", &referenceTag);
if (focusedTag < 1) {
focusedTag = 1;
} else if (focusedTag > 16) {
focusedTag = 16;
}
if (referenceTag < 1) {
referenceTag = 1;
} else if (referenceTag > 16) {
referenceTag = 16;
}
if (!calibration_json_path.empty() && !selected_field_map.empty()) {
std::ifstream calJson(calibration_json_path);
std::ifstream refJson(selected_field_map);
currentCalibrationMap = Fieldmap(wpi::json::parse(calJson));
currentReferenceMap = Fieldmap(wpi::json::parse(refJson));
double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
currentCalibrationMap.getTag(focusedTag).yawRot;
double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
currentCalibrationMap.getTag(focusedTag).pitchRot;
double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
currentCalibrationMap.getTag(focusedTag).rollRot;
double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
currentCalibrationMap.getTag(focusedTag).xPos;
double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
currentCalibrationMap.getTag(focusedTag).yPos;
double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
currentCalibrationMap.getTag(focusedTag).zPos;
ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str());
ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str());
ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str());
ImGui::TextWrapped(
"Yaw Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Pitch Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::TextWrapped(
"Roll Difference %s°",
std::to_string(
Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
.c_str());
ImGui::NewLine();
ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
}
if (ImGui::Button("Close")) {
ImGui::CloseCurrentPopup();
}
ImGui::EndPopup();
}
ImGui::End();
}
#ifndef RUNNING_WPICAL_TESTS
#ifdef _WIN32
int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine,
int nCmdShow) {
int argc = __argc;
char** argv = __argv;
#else
int main(int argc, char** argv) {
#endif
std::string_view saveDir;
if (argc == 2) {
saveDir = argv[1];
}
gui::CreateContext();
gui::AddIcon(wpical::GetResource_wpical_16_png());
gui::AddIcon(wpical::GetResource_wpical_32_png());
gui::AddIcon(wpical::GetResource_wpical_48_png());
gui::AddIcon(wpical::GetResource_wpical_64_png());
gui::AddIcon(wpical::GetResource_wpical_128_png());
gui::AddIcon(wpical::GetResource_wpical_256_png());
gui::AddIcon(wpical::GetResource_wpical_512_png());
gui::AddLateExecute(DisplayGui);
gui::Initialize("wpical", 600, 400);
gui::Main();
gui::DestroyContext();
return 0;
}
#endif

404
wpical/src/main/native/cpp/cameracalibration.cpp Normal file
View File

@@ -0,0 +1,404 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "cameracalibration.h"
#include <fstream>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <mrcal_wrapper.h>
#include <opencv2/objdetect/aruco_board.hpp>
#include <opencv2/opencv.hpp>
#include <opencv2/videoio.hpp>
static bool filter(std::vector<cv::Point2f> charuco_corners,
std::vector<int> charuco_ids,
std::vector<std::vector<cv::Point2f>> marker_corners,
std::vector<int> marker_ids, int board_width,
int board_height) {
if (charuco_ids.empty() || charuco_corners.empty()) {
return false;
}
if (charuco_corners.size() < 10 || charuco_ids.size() < 10) {
return false;
}
for (int i = 0; i < charuco_ids.size(); i++) {
if (charuco_ids.at(i) > (board_width - 1) * (board_height - 1) - 1) {
return false;
}
}
for (int i = 0; i < marker_ids.size(); i++) {
if (marker_ids.at(i) > ((board_width * board_height) / 2) - 1) {
return false;
}
}
return true;
}
int cameracalibration::calibrate(const std::string& input_video,
CameraModel& camera_model, float square_width,
float marker_width, int board_width,
int board_height, bool show_debug_window) {
// ChArUco Board
cv::aruco::Dictionary aruco_dict =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
cv::Ptr<cv::aruco::CharucoBoard> charuco_board = new cv::aruco::CharucoBoard(
cv::Size(board_width, board_height), square_width * 0.0254,
marker_width * 0.0254, aruco_dict);
cv::aruco::CharucoDetector charuco_detector(*charuco_board);
// Video capture
cv::VideoCapture video_capture(input_video);
cv::Size frame_shape;
std::vector<std::vector<cv::Point2f>> all_charuco_corners;
std::vector<std::vector<int>> all_charuco_ids;
std::vector<std::vector<cv::Point3f>> all_obj_points;
std::vector<std::vector<cv::Point2f>> all_img_points;
while (video_capture.grab()) {
cv::Mat frame;
video_capture.retrieve(frame);
cv::Mat debug_image = frame;
if (frame.empty()) {
break;
}
// Detect
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
frame_shape = frame_gray.size();
std::vector<cv::Point2f> charuco_corners;
std::vector<int> charuco_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;
std::vector<int> marker_ids;
std::vector<cv::Point3f> obj_points;
std::vector<cv::Point2f> img_points;
charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids,
marker_corners, marker_ids);
if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids,
board_width, board_height)) {
continue;
}
charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points,
img_points);
all_charuco_corners.push_back(charuco_corners);
all_charuco_ids.push_back(charuco_ids);
all_obj_points.push_back(obj_points);
all_img_points.push_back(img_points);
if (show_debug_window) {
cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids);
cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners,
charuco_ids);
cv::imshow("Frame", debug_image);
if (cv::waitKey(1) == 'q') {
break;
}
}
}
video_capture.release();
if (show_debug_window) {
cv::destroyAllWindows();
}
// Calibrate
cv::Mat camera_matrix, dist_coeffs;
std::vector<cv::Mat> r_vecs, t_vecs;
std::vector<double> std_dev_intrinsics, std_dev_extrinsics, per_view_errors;
double repError;
try {
// see https://stackoverflow.com/a/75865177
int flags = cv::CALIB_RATIONAL_MODEL | cv::CALIB_USE_LU;
repError = cv::calibrateCamera(
all_obj_points, all_img_points, frame_shape, camera_matrix, dist_coeffs,
r_vecs, t_vecs, cv::noArray(), cv::noArray(), cv::noArray(), flags);
} catch (...) {
std::cout << "calibration failed" << std::endl;
return 1;
}
std::vector<double> matrix = {camera_matrix.begin<double>(),
camera_matrix.end<double>()};
std::vector<double> distortion = {dist_coeffs.begin<double>(),
dist_coeffs.end<double>()};
camera_model.intrinsic_matrix = Eigen::Matrix<double, 3, 3>(matrix.data());
camera_model.distortion_coefficients =
Eigen::Matrix<double, 8, 1>(distortion.data());
camera_model.avg_reprojection_error = repError;
return 0;
}
int cameracalibration::calibrate(const std::string& input_video,
CameraModel& camera_model, float square_width,
float marker_width, int board_width,
int board_height, double imagerWidthPixels,
double imagerHeightPixels,
bool show_debug_window) {
// ChArUco Board
cv::aruco::Dictionary aruco_dict =
cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
cv::Ptr<cv::aruco::CharucoBoard> charuco_board = new cv::aruco::CharucoBoard(
cv::Size(board_width, board_height), square_width * 0.0254,
marker_width * 0.0254, aruco_dict);
cv::aruco::CharucoDetector charuco_detector(*charuco_board);
// Video capture
cv::VideoCapture video_capture(input_video);
cv::Size frame_shape;
// Detection output
std::vector<mrcal_point3_t> observation_boards;
std::vector<mrcal_pose_t> frames_rt_toref;
cv::Size boardSize(board_width - 1, board_height - 1);
cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels);
while (video_capture.grab()) {
cv::Mat frame;
video_capture.retrieve(frame);
cv::Mat debug_image = frame;
if (frame.empty()) {
break;
}
// Detect
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
frame_shape = frame_gray.size();
std::vector<cv::Point2f> charuco_corners;
std::vector<int> charuco_ids;
std::vector<std::vector<cv::Point2f>> marker_corners;
std::vector<int> marker_ids;
std::vector<cv::Point3f> obj_points;
std::vector<cv::Point2f> img_points;
std::vector<mrcal_point3_t> points((board_width - 1) * (board_height - 1));
charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids,
marker_corners, marker_ids);
if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids,
board_width, board_height)) {
continue;
}
charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points,
img_points);
for (int i = 0; i < charuco_ids.size(); i++) {
int id = charuco_ids.at(i);
points[id].x = img_points.at(i).x;
points[id].y = img_points.at(i).y;
points[id].z = 1.0f;
}
for (int i = 0; i < points.size(); i++) {
if (points[i].z != 1.0f) {
points[i].x = -1.0f;
points[i].y = -1.0f;
points[i].z = -1.0f;
}
}
frames_rt_toref.push_back(
getSeedPose(points.data(), boardSize, imagerSize, square_width, 1000));
observation_boards.insert(observation_boards.end(), points.begin(),
points.end());
if (show_debug_window) {
cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids);
cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners,
charuco_ids);
cv::imshow("Frame", debug_image);
if (cv::waitKey(1) == 'q') {
break;
}
}
}
video_capture.release();
if (show_debug_window) {
cv::destroyAllWindows();
}
if (observation_boards.empty()) {
std::cout << "calibration failed" << std::endl;
return 1;
} else {
std::cout << "points detected" << std::endl;
}
std::unique_ptr<mrcal_result> cal_result =
mrcal_main(observation_boards, frames_rt_toref, boardSize,
square_width * 0.0254, imagerSize, 1000);
auto& stats = *cal_result;
// Camera matrix and distortion coefficients
std::vector<double> camera_matrix = {
// fx 0 cx
stats.intrinsics[0], 0, stats.intrinsics[2],
// 0 fy cy
0, stats.intrinsics[1], stats.intrinsics[3],
// 0 0 1
0, 0, 1};
std::vector<double> distortion_coefficients = {stats.intrinsics[4],
stats.intrinsics[5],
stats.intrinsics[6],
stats.intrinsics[7],
stats.intrinsics[8],
stats.intrinsics[9],
stats.intrinsics[10],
stats.intrinsics[11],
0.0,
0.0,
0.0,
0.0,
0.0,
0.0};
camera_model.intrinsic_matrix =
Eigen::Matrix<double, 3, 3>(camera_matrix.data());
camera_model.distortion_coefficients =
Eigen::Matrix<double, 8, 1>(distortion_coefficients.data());
camera_model.avg_reprojection_error = stats.rms_error;
return 0;
}
int cameracalibration::calibrate(const std::string& input_video,
CameraModel& camera_model, float square_width,
int board_width, int board_height,
double imagerWidthPixels,
double imagerHeightPixels,
bool show_debug_window) {
// Video capture
cv::VideoCapture video_capture(input_video);
// Detection output
std::vector<mrcal_point3_t> observation_boards;
std::vector<mrcal_pose_t> frames_rt_toref;
cv::Size boardSize(board_width - 1, board_height - 1);
cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels);
while (video_capture.grab()) {
cv::Mat frame;
video_capture.retrieve(frame);
if (frame.empty()) {
break;
}
// Detect
cv::Mat frame_gray;
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
std::vector<cv::Point2f> corners;
bool found = cv::findChessboardCorners(
frame_gray, boardSize, corners,
cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
if (found) {
std::vector<mrcal_point3_t> current_points;
for (int i = 0; i < corners.size(); i++) {
current_points.push_back(
mrcal_point3_t{{corners.at(i).x, corners.at(i).y, 1.0f}});
}
frames_rt_toref.push_back(getSeedPose(current_points.data(), boardSize,
imagerSize, square_width * 0.0254,
1000));
observation_boards.insert(observation_boards.end(),
current_points.begin(), current_points.end());
}
if (show_debug_window) {
cv::drawChessboardCorners(frame, boardSize, corners, found);
cv::imshow("Checkerboard Detection", frame);
if (cv::waitKey(30) == 'q') {
break;
}
}
}
video_capture.release();
if (show_debug_window) {
cv::destroyAllWindows();
}
if (observation_boards.empty()) {
std::cout << "calibration failed" << std::endl;
return 1;
} else {
std::cout << "points detected" << std::endl;
}
std::unique_ptr<mrcal_result> cal_result =
mrcal_main(observation_boards, frames_rt_toref, boardSize,
square_width * 0.0254, imagerSize, 1000);
auto& stats = *cal_result;
// Camera matrix and distortion coefficients
std::vector<double> camera_matrix = {
// fx 0 cx
stats.intrinsics[0], 0, stats.intrinsics[2],
// 0 fy cy
0, stats.intrinsics[1], stats.intrinsics[3],
// 0 0 1
0, 0, 1};
std::vector<double> distortion_coefficients = {stats.intrinsics[4],
stats.intrinsics[5],
stats.intrinsics[6],
stats.intrinsics[7],
stats.intrinsics[8],
stats.intrinsics[9],
stats.intrinsics[10],
stats.intrinsics[11],
0.0,
0.0,
0.0,
0.0,
0.0,
0.0};
// Save calibration output
camera_model.intrinsic_matrix =
Eigen::Matrix<double, 3, 3>(camera_matrix.data());
camera_model.distortion_coefficients =
Eigen::Matrix<double, 8, 1>(distortion_coefficients.data());
camera_model.avg_reprojection_error = stats.rms_error;
return 0;
}

599
wpical/src/main/native/cpp/fieldcalibration.cpp Normal file
View File

@@ -0,0 +1,599 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "fieldcalibration.h"
#include <algorithm>
#include <filesystem>
#include <fstream>
#include <functional>
#include <iostream>
#include <map>
#include <string>
#include <utility>
#include <vector>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <ceres/ceres.h>
#include <opencv2/calib3d.hpp>
#include <opencv2/core/eigen.hpp>
#include <opencv2/core/utils/logger.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/videoio.hpp>
#include <wpi/json.h>
#include "apriltag.h"
#include "tag36h11.h"
struct Pose {
Eigen::Vector3d p;
Eigen::Quaterniond q;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
struct Constraint {
int id_begin;
int id_end;
Pose t_begin_end;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};
class PoseGraphError {
public:
explicit PoseGraphError(Pose t_ab_observed)
: m_t_ab_observed(std::move(t_ab_observed)) {}
template <typename T>
bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
const T* const p_b_ptr, const T* const q_b_ptr,
T* residuals_ptr) const {
// Tag A
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
// Tag B
Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);
// Rotation between tag A to tag B
Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
// Displacement between tag A and tag B in tag A's frame
Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
// Error between the orientations
Eigen::Quaternion<T> delta_q =
m_t_ab_observed.q.template cast<T>() * q_ab_estimated.conjugate();
// Residuals
Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
residuals.template block<3, 1>(0, 0) =
p_ab_estimated - m_t_ab_observed.p.template cast<T>();
residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
return true;
}
static ceres::CostFunction* Create(const Pose& t_ab_observed) {
return new ceres::AutoDiffCostFunction<PoseGraphError, 6, 3, 4, 3, 4>(
new PoseGraphError(t_ab_observed));
}
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
private:
const Pose m_t_ab_observed;
};
const double tagSizeMeters = 0.1651;
inline cameracalibration::CameraModel load_camera_model(std::string path) {
Eigen::Matrix<double, 3, 3> camera_matrix;
Eigen::Matrix<double, 8, 1> camera_distortion;
std::ifstream file(path);
wpi::json json_data;
try {
json_data = wpi::json::parse(file);
} catch (const wpi::json::parse_error& e) {
std::cout << e.what() << std::endl;
}
bool isCalibdb = json_data.contains("camera");
if (!isCalibdb) {
for (int i = 0; i < camera_matrix.rows(); i++) {
for (int j = 0; j < camera_matrix.cols(); j++) {
camera_matrix(i, j) =
json_data["camera_matrix"][(i * camera_matrix.cols()) + j];
}
}
for (int i = 0; i < camera_distortion.rows(); i++) {
for (int j = 0; j < camera_distortion.cols(); j++) {
camera_distortion(i, j) = json_data["distortion_coefficients"]
[(i * camera_distortion.cols()) + j];
}
}
} else {
for (int i = 0; i < camera_matrix.rows(); i++) {
for (int j = 0; j < camera_matrix.cols(); j++) {
try {
camera_matrix(i, j) = json_data["camera_matrix"][i][j];
} catch (...) {
camera_matrix(i, j) = json_data["camera_matrix"]["data"]
[(i * camera_matrix.cols()) + j];
}
}
}
for (int i = 0; i < camera_distortion.rows(); i++) {
for (int j = 0; j < camera_distortion.cols(); j++) {
try {
camera_distortion(i, j) =
json_data["distortion_coefficients"]
[(i * camera_distortion.cols()) + j];
} catch (...) {
camera_distortion(i, j) = 0.0;
}
}
}
}
cameracalibration::CameraModel camera_model{
camera_matrix, camera_distortion, json_data["avg_reprojection_error"]};
return camera_model;
}
inline cameracalibration::CameraModel load_camera_model(wpi::json json_data) {
// Camera matrix
Eigen::Matrix<double, 3, 3> camera_matrix;
for (int i = 0; i < camera_matrix.rows(); i++) {
for (int j = 0; j < camera_matrix.cols(); j++) {
camera_matrix(i, j) =
json_data["camera_matrix"][(i * camera_matrix.cols()) + j];
}
}
// Distortion coefficients
Eigen::Matrix<double, 8, 1> camera_distortion;
for (int i = 0; i < camera_distortion.rows(); i++) {
for (int j = 0; j < camera_distortion.cols(); j++) {
camera_distortion(i, j) = json_data["distortion_coefficients"]
[(i * camera_distortion.cols()) + j];
}
}
cameracalibration::CameraModel camera_model{
camera_matrix, camera_distortion, json_data["avg_reprojection_error"]};
return camera_model;
}
inline std::map<int, wpi::json> load_ideal_map(std::string path) {
std::ifstream file(path);
wpi::json json_data = wpi::json::parse(file);
std::map<int, wpi::json> ideal_map;
for (const auto& element : json_data["tags"]) {
ideal_map[element["ID"]] = element;
}
return ideal_map;
}
Eigen::Matrix<double, 4, 4> get_tag_transform(
std::map<int, wpi::json>& ideal_map, int tag_id) {
Eigen::Matrix<double, 4, 4> transform =
Eigen::Matrix<double, 4, 4>::Identity();
transform.block<3, 3>(0, 0) =
Eigen::Quaternion<double>(
ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["W"],
ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["X"],
ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"],
ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"])
.toRotationMatrix();
transform(0, 3) = ideal_map[tag_id]["pose"]["translation"]["x"];
transform(1, 3) = ideal_map[tag_id]["pose"]["translation"]["y"];
transform(2, 3) = ideal_map[tag_id]["pose"]["translation"]["z"];
return transform;
}
inline Eigen::Matrix<double, 4, 4> estimate_tag_pose(
apriltag_detection_t* tag_detection,
const Eigen::Matrix<double, 3, 3>& camera_matrix,
const Eigen::Matrix<double, 8, 1>& camera_distortion, double tag_size) {
cv::Mat camera_matrix_cv;
cv::Mat camera_distortion_cv;
cv::eigen2cv(camera_matrix, camera_matrix_cv);
cv::eigen2cv(camera_distortion, camera_distortion_cv);
std::vector<cv::Point2f> points_2d = {
cv::Point2f(tag_detection->p[0][0], tag_detection->p[0][1]),
cv::Point2f(tag_detection->p[1][0], tag_detection->p[1][1]),
cv::Point2f(tag_detection->p[2][0], tag_detection->p[2][1]),
cv::Point2f(tag_detection->p[3][0], tag_detection->p[3][1])};
std::vector<cv::Point3f> points_3d_box_base = {
cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0),
cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0),
cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0),
cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)};
std::vector<double> r_vec;
std::vector<double> t_vec;
cv::solvePnP(points_3d_box_base, points_2d, camera_matrix_cv,
camera_distortion_cv, r_vec, t_vec, false, cv::SOLVEPNP_SQPNP);
cv::Mat r_mat;
cv::Rodrigues(r_vec, r_mat);
Eigen::Matrix<double, 4, 4> camera_to_tag{
{r_mat.at<double>(0, 0), r_mat.at<double>(0, 1), r_mat.at<double>(0, 2),
t_vec.at(0)},
{r_mat.at<double>(1, 0), r_mat.at<double>(1, 1), r_mat.at<double>(1, 2),
t_vec.at(1)},
{r_mat.at<double>(2, 0), r_mat.at<double>(2, 1), r_mat.at<double>(2, 2),
t_vec.at(2)},
{0.0, 0.0, 0.0, 1.0}};
return camera_to_tag;
}
inline void draw_tag_cube(cv::Mat& frame,
Eigen::Matrix<double, 4, 4> camera_to_tag,
const Eigen::Matrix<double, 3, 3>& camera_matrix,
const Eigen::Matrix<double, 8, 1>& camera_distortion,
double tag_size) {
cv::Mat camera_matrix_cv;
cv::Mat camera_distortion_cv;
cv::eigen2cv(camera_matrix, camera_matrix_cv);
cv::eigen2cv(camera_distortion, camera_distortion_cv);
std::vector<cv::Point3f> points_3d_box_base = {
cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0),
cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0),
cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0),
cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)};
std::vector<cv::Point3f> points_3d_box_top = {
cv::Point3f(-tag_size / 2.0, tag_size / 2.0, -tag_size),
cv::Point3f(tag_size / 2.0, tag_size / 2.0, -tag_size),
cv::Point3f(tag_size / 2.0, -tag_size / 2.0, -tag_size),
cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, -tag_size)};
std::vector<cv::Point2f> points_2d_box_base = {
cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0),
cv::Point2f(0.0, 0.0)};
std::vector<cv::Point2f> points_2d_box_top = {
cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0),
cv::Point2f(0.0, 0.0)};
Eigen::Matrix<double, 3, 3> r_vec = camera_to_tag.block<3, 3>(0, 0);
Eigen::Matrix<double, 3, 1> t_vec = camera_to_tag.block<3, 1>(0, 3);
cv::Mat r_vec_cv;
cv::Mat t_vec_cv;
cv::eigen2cv(r_vec, r_vec_cv);
cv::eigen2cv(t_vec, t_vec_cv);
cv::projectPoints(points_3d_box_base, r_vec_cv, t_vec_cv, camera_matrix_cv,
camera_distortion_cv, points_2d_box_base);
cv::projectPoints(points_3d_box_top, r_vec_cv, t_vec_cv, camera_matrix_cv,
camera_distortion_cv, points_2d_box_top);
for (int i = 0; i < 4; i++) {
cv::Point2f& point_base = points_2d_box_base.at(i);
cv::Point2f& point_top = points_2d_box_top.at(i);
cv::line(frame, point_base, point_top, cv::Scalar(0, 255, 255), 5);
int i_next = (i + 1) % 4;
cv::Point2f& point_base_next = points_2d_box_base.at(i_next);
cv::Point2f& point_top_next = points_2d_box_top.at(i_next);
cv::line(frame, point_base, point_base_next, cv::Scalar(0, 255, 255), 5);
cv::line(frame, point_top, point_top_next, cv::Scalar(0, 255, 255), 5);
}
}
inline bool process_video_file(
apriltag_detector_t* tag_detector,
const Eigen::Matrix<double, 3, 3>& camera_matrix,
const Eigen::Matrix<double, 8, 1>& camera_distortion, double tag_size,
const std::string& path,
std::map<int, Pose, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose>>>& poses,
std::vector<Constraint, Eigen::aligned_allocator<Constraint>>& constraints,
bool show_debug_window) {
if (show_debug_window) {
cv::namedWindow("Processing Frame", cv::WINDOW_NORMAL);
}
cv::VideoCapture video_input(path);
if (!video_input.isOpened()) {
std::cout << "Unable to open video " << path << std::endl;
return false;
}
cv::Mat frame;
cv::Mat frame_gray;
cv::Mat frame_debug;
int frame_num = 0;
while (video_input.read(frame)) {
std::cout << "Processing " << path << " - Frame " << frame_num++
<< std::endl;
// Convert color frame to grayscale frame
cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
// Clone color frame for debugging
frame_debug = frame.clone();
// Detect tags
image_u8_t tag_image = {frame_gray.cols, frame_gray.rows, frame_gray.cols,
frame_gray.data};
zarray_t* tag_detections =
apriltag_detector_detect(tag_detector, &tag_image);
// Skip this loop if there are no tags detected
if (zarray_size(tag_detections) == 0) {
std::cout << "No tags detected" << std::endl;
continue;
}
// Find detection with the smallest tag ID
apriltag_detection_t* tag_detection_min = nullptr;
zarray_get(tag_detections, 0, &tag_detection_min);
for (int i = 0; i < zarray_size(tag_detections); i++) {
apriltag_detection_t* tag_detection_i;
zarray_get(tag_detections, i, &tag_detection_i);
if (tag_detection_i->id < tag_detection_min->id) {
tag_detection_min = tag_detection_i;
}
}
Eigen::Matrix<double, 4, 4> camera_to_tag_min = estimate_tag_pose(
tag_detection_min, camera_matrix, camera_distortion, tag_size);
// Find transformation from smallest tag ID
for (int i = 0; i < zarray_size(tag_detections); i++) {
apriltag_detection_t* tag_detection_i;
zarray_get(tag_detections, i, &tag_detection_i);
// Add tag ID to poses
if (poses.find(tag_detection_i->id) == poses.end()) {
poses[tag_detection_i->id] = {Eigen::Vector3d(0.0, 0.0, 0.0),
Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)};
}
// Estimate camera to tag pose
Eigen::Matrix<double, 4, 4> caamera_to_tag = estimate_tag_pose(
tag_detection_i, camera_matrix, camera_distortion, tag_size);
// Draw debug cube
if (show_debug_window) {
draw_tag_cube(frame_debug, caamera_to_tag, camera_matrix,
camera_distortion, tag_size);
}
// Skip finding transformation from smallest tag ID to itself
if (tag_detection_i->id == tag_detection_min->id) {
continue;
}
Eigen::Matrix<double, 4, 4> tag_min_to_tag =
camera_to_tag_min.inverse() * caamera_to_tag;
// Constraint
Constraint constraint;
constraint.id_begin = tag_detection_min->id;
constraint.id_end = tag_detection_i->id;
constraint.t_begin_end.p = tag_min_to_tag.block<3, 1>(0, 3);
constraint.t_begin_end.q =
Eigen::Quaterniond(tag_min_to_tag.block<3, 3>(0, 0));
constraints.push_back(constraint);
}
apriltag_detections_destroy(tag_detections);
// Show debug
if (show_debug_window) {
cv::imshow("Processing Frame", frame_debug);
cv::waitKey(1);
}
}
video_input.release();
if (show_debug_window) {
cv::destroyAllWindows();
}
return true;
}
int fieldcalibration::calibrate(std::string input_dir_path,
std::string output_file_path,
std::string camera_model_path,
std::string ideal_map_path, int pinned_tag_id,
bool show_debug_window) {
// Silence OpenCV logging
cv::utils::logging::setLogLevel(
cv::utils::logging::LogLevel::LOG_LEVEL_SILENT);
// Load camera model
Eigen::Matrix3d camera_matrix;
Eigen::Matrix<double, 8, 1> camera_distortion;
try {
auto camera_model = load_camera_model(camera_model_path);
camera_matrix = camera_model.intrinsic_matrix;
camera_distortion = camera_model.distortion_coefficients;
} catch (...) {
return 1;
}
wpi::json json = wpi::json::parse(std::ifstream(ideal_map_path));
if (!json.contains("tags")) {
return 1;
}
// Load ideal field map
std::map<int, wpi::json> ideal_map;
try {
ideal_map = load_ideal_map(ideal_map_path);
} catch (...) {
return 1;
}
// Apriltag detector
apriltag_detector_t* tag_detector = apriltag_detector_create();
tag_detector->nthreads = 8;
apriltag_family_t* tag_family = tag36h11_create();
apriltag_detector_add_family(tag_detector, tag_family);
// Find tag poses
std::map<int, Pose, std::less<int>,
Eigen::aligned_allocator<std::pair<const int, Pose>>>
poses;
std::vector<Constraint, Eigen::aligned_allocator<Constraint>> constraints;
for (const auto& entry :
std::filesystem::directory_iterator(input_dir_path)) {
if (entry.path().filename().string()[0] == '.') {
continue;
}
const std::string path = entry.path().string();
bool success = process_video_file(tag_detector, camera_matrix,
camera_distortion, tagSizeMeters, path,
poses, constraints, show_debug_window);
if (!success) {
std::cout << "Unable to process video " << path << std::endl;
return 1;
}
}
// Build optimization problem
ceres::Problem problem;
ceres::Manifold* quaternion_manifold = new ceres::EigenQuaternionManifold;
for (const auto& constraint : constraints) {
auto pose_begin_iter = poses.find(constraint.id_begin);
auto pose_end_iter = poses.find(constraint.id_end);
ceres::CostFunction* cost_function =
PoseGraphError::Create(constraint.t_begin_end);
problem.AddResidualBlock(cost_function, nullptr,
pose_begin_iter->second.p.data(),
pose_begin_iter->second.q.coeffs().data(),
pose_end_iter->second.p.data(),
pose_end_iter->second.q.coeffs().data());
problem.SetManifold(pose_begin_iter->second.q.coeffs().data(),
quaternion_manifold);
problem.SetManifold(pose_end_iter->second.q.coeffs().data(),
quaternion_manifold);
}
// Pin tag
auto pinned_tag_iter = poses.find(pinned_tag_id);
if (pinned_tag_iter != poses.end()) {
problem.SetParameterBlockConstant(pinned_tag_iter->second.p.data());
problem.SetParameterBlockConstant(
pinned_tag_iter->second.q.coeffs().data());
}
// Solve
ceres::Solver::Options options;
options.max_num_iterations = 200;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.num_threads = 10;
ceres::Solver::Summary summary;
ceres::Solve(options, &problem, &summary);
std::cout << summary.BriefReport() << std::endl;
// Output
std::map<int, wpi::json> observed_map = ideal_map;
Eigen::Matrix<double, 4, 4> correction_a;
correction_a << 0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;
Eigen::Matrix<double, 4, 4> correction_b;
correction_b << 0, 1, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 1;
Eigen::Matrix<double, 4, 4> pinned_tag_transform =
get_tag_transform(ideal_map, pinned_tag_id);
for (const auto& [tag_id, pose] : poses) {
// Transformation from pinned tag
Eigen::Matrix<double, 4, 4> transform =
Eigen::Matrix<double, 4, 4>::Identity();
transform.block<3, 3>(0, 0) = pose.q.toRotationMatrix();
transform.block<3, 1>(0, 3) = pose.p;
// Transformation from world
Eigen::Matrix<double, 4, 4> corrected_transform =
pinned_tag_transform * correction_a * transform * correction_b;
Eigen::Quaternion<double> corrected_transform_q(
corrected_transform.block<3, 3>(0, 0));
observed_map[tag_id]["pose"]["translation"]["x"] =
corrected_transform(0, 3);
observed_map[tag_id]["pose"]["translation"]["y"] =
corrected_transform(1, 3);
observed_map[tag_id]["pose"]["translation"]["z"] =
corrected_transform(2, 3);
observed_map[tag_id]["pose"]["rotation"]["quaternion"]["X"] =
corrected_transform_q.x();
observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"] =
corrected_transform_q.y();
observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"] =
corrected_transform_q.z();
observed_map[tag_id]["pose"]["rotation"]["quaternion"]["W"] =
corrected_transform_q.w();
}
wpi::json observed_map_json;
for (const auto& [tag_id, tag_json] : observed_map) {
observed_map_json["tags"].push_back(tag_json);
}
observed_map_json["field"] = {
{"length", static_cast<double>(json.at("field").at("length"))},
{"width", static_cast<double>(json.at("field").at("width"))}};
std::ofstream output_file(output_file_path);
output_file << observed_map_json.dump(4) << std::endl;
return 0;
}

40
wpical/src/main/native/cpp/fmap.cpp Normal file
View File

@@ -0,0 +1,40 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include "fmap.h"
#include <string>
#include <vector>
wpi::json fmap::singleTag(int tag, const tag::Pose& tagpose) {
std::vector<double> transform = {};
for (int i = 0; i < 4; i++) {
for (int j = 0; j < 4; j++) {
transform.push_back(tagpose.transformMatrixFmap(i, j));
}
}
return {{"family", "apriltag3_36h11_classic"},
{"id", tag},
{"size", 165.1},
{"transform", transform},
{"unique", true}};
}
wpi::json fmap::convertfmap(const wpi::json& json) {
std::string fmapstart = "{\"fiducials\":[";
std::string fmapend = "],\"type\":\"frc\"}";
Fieldmap fieldmap(json);
for (int i = 0; i < fieldmap.getNumTags(); i++) {
fmapstart += singleTag(i + 1, fieldmap.getTag(i + 1)).dump();
if (i != fieldmap.getNumTags() - 1) {
fmapstart += ",";
}
}
return wpi::json::parse(fmapstart.append(fmapend));
}

29
wpical/src/main/native/cpp/tagpose.cpp Normal file
View File

@@ -0,0 +1,29 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <tagpose.h>
namespace tag {
Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
double z, double field_length_meters, double field_width_meters) {
xPos = xpos;
yPos = ypos;
zPos = zpos;
quaternion = Eigen::Quaterniond(w, x, y, z);
rotationMatrix = quaternion.toRotationMatrix();
transformMatrixFmap.setZero();
transformMatrixFmap.block<3, 3>(0, 0) = rotationMatrix;
transformMatrixFmap(0, 3) = xpos - (field_length_meters / 2.0);
transformMatrixFmap(1, 3) = ypos - (field_width_meters / 2.0);
transformMatrixFmap(2, 3) = zpos;
transformMatrixFmap(3, 3) = 1;
transformMatrixFmap(3, 0) = 0;
transformMatrixFmap(3, 1) = 0;
transformMatrixFmap(3, 2) = 0;
Eigen::Vector3d eulerAngles = rotationMatrix.eulerAngles(0, 1, 2);
rollRot = eulerAngles[0];
pitchRot = eulerAngles[1];
yawRot = eulerAngles[2];
}
} // namespace tag

51
wpical/src/main/native/include/cameracalibration.h Normal file
View File

@@ -0,0 +1,51 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <fstream>
#include <string>
#include <vector>
#include <Eigen/Core>
#include <wpi/json.h>
namespace cameracalibration {
struct CameraModel {
Eigen::Matrix<double, 3, 3> intrinsic_matrix;
Eigen::Matrix<double, 8, 1> distortion_coefficients;
double avg_reprojection_error;
};
int calibrate(const std::string& input_video, CameraModel& camera_model,
float square_width, float marker_width, int board_width,
int board_height, bool show_debug_window);
int calibrate(const std::string& input_video, CameraModel& camera_model,
float square_width, float marker_width, int board_width,
int board_height, double imagerWidthPixels,
double imagerHeightPixels, bool show_debug_window);
int calibrate(const std::string& input_video, CameraModel& camera_model,
float square_width, int board_width, int board_height,
double imagerWidthPixels, double imagerHeightPixels,
bool show_debug_window);
static void dumpJson(CameraModel& camera_model,
const std::string& output_file_path) {
std::vector<double> camera_matrix(camera_model.intrinsic_matrix.data(),
camera_model.intrinsic_matrix.data() +
camera_model.intrinsic_matrix.size());
std::vector<double> distortion_coefficients(
camera_model.distortion_coefficients.data(),
camera_model.distortion_coefficients.data() +
camera_model.distortion_coefficients.size());
wpi::json result = {
{"camera_matrix", camera_matrix},
{"distortion_coefficients", distortion_coefficients},
{"avg_reprojection_error", camera_model.avg_reprojection_error}};
std::ofstream output_file(output_file_path);
output_file << result.dump(4) << std::endl;
output_file.close();
}
} // namespace cameracalibration

15
wpical/src/main/native/include/fieldcalibration.h Normal file
View File

@@ -0,0 +1,15 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <string>
#include "cameracalibration.h"
namespace fieldcalibration {
int calibrate(std::string input_dir_path, std::string output_file_path,
std::string camera_model_path, std::string ideal_map_path,
int pinned_tag_id, bool show_debug_window);
} // namespace fieldcalibration

59
wpical/src/main/native/include/fieldmap.h Normal file
View File

@@ -0,0 +1,59 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <cmath>
#include <vector>
#include <tagpose.h>
#include <wpi/json.h>
class Fieldmap {
public:
Fieldmap() = default;
explicit Fieldmap(const wpi::json& json) {
double field_length_meters =
static_cast<double>(json.at("field").at("length"));
double field_width_meters =
static_cast<double>(json.at("field").at("width"));
for (const auto& tag : json.at("tags").items()) {
double tagXPos =
static_cast<double>(tag.value().at("pose").at("translation").at("x"));
double tagYPos =
static_cast<double>(tag.value().at("pose").at("translation").at("y"));
double tagZPos =
static_cast<double>(tag.value().at("pose").at("translation").at("z"));
double tagWQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("W"));
double tagXQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("X"));
double tagYQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("Y"));
double tagZQuat = static_cast<double>(
tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
tagYQuat, tagZQuat, field_length_meters,
field_width_meters);
}
}
const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
int getNumTags() const { return tagVec.size(); }
static double minimizeAngle(double angle) {
angle = std::fmod(angle, 360);
if (angle > 180) {
return angle - 360;
} else if (angle < -180) {
return angle + 360;
}
return angle;
}
private:
std::vector<tag::Pose> tagVec;
};

15
wpical/src/main/native/include/fmap.h Normal file
View File

@@ -0,0 +1,15 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <fieldmap.h>
#include <tagpose.h>
#include <wpi/json.h>
namespace fmap {
wpi::json singleTag(int tag, const tag::Pose& tagpose);
wpi::json convertfmap(const wpi::json& json);
} // namespace fmap

20
wpical/src/main/native/include/tagpose.h Normal file
View File

@@ -0,0 +1,20 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace tag {
class Pose {
public:
Pose(double xpos, double ypos, double zpos, double w, double x, double y,
double z, double field_length_meters, double field_width_meters);
double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
Eigen::Quaterniond quaternion;
Eigen::Matrix3d rotationMatrix;
Eigen::Matrix4d transformMatrixFmap;
};
} // namespace tag

BIN
wpical/src/main/native/mac/wpical.icns Normal file
View File

Binary file not shown.

296
wpical/src/main/native/resources/2024-crescendo.json Normal file
View File

@@ -0,0 +1,296 @@
{
"tags": [
{
"ID": 1,
"pose": {
"translation": {
"x": 15.079471999999997,
"y": 0.24587199999999998,
"z": 1.355852
},
"rotation": {
"quaternion": {
"W": 0.5000000000000001,
"X": 0.0,
"Y": 0.0,
"Z": 0.8660254037844386
}
}
}
},
{
"ID": 2,
"pose": {
"translation": {
"x": 16.185134,
"y": 0.883666,
"z": 1.355852
},
"rotation": {
"quaternion": {
"W": 0.5000000000000001,
"X": 0.0,
"Y": 0.0,
"Z": 0.8660254037844386
}
}
}
},
{
"ID": 3,
"pose": {
"translation": {
"x": 16.579342,
"y": 4.982717999999999,
"z": 1.4511020000000001
},
"rotation": {
"quaternion": {
"W": 6.123233995736766e-17,
"X": 0.0,
"Y": 0.0,
"Z": 1.0
}
}
}
},
{
"ID": 4,
"pose": {
"translation": {
"x": 16.579342,
"y": 5.547867999999999,
"z": 1.4511020000000001
},
"rotation": {
"quaternion": {
"W": 6.123233995736766e-17,
"X": 0.0,
"Y": 0.0,
"Z": 1.0
}
}
}
},
{
"ID": 5,
"pose": {
"translation": {
"x": 14.700757999999999,
"y": 8.2042,
"z": 1.355852
},
"rotation": {
"quaternion": {
"W": -0.7071067811865475,
"X": -0.0,
"Y": 0.0,
"Z": 0.7071067811865476
}
}
}
},
{
"ID": 6,
"pose": {
"translation": {
"x": 1.8415,
"y": 8.2042,
"z": 1.355852
},
"rotation": {
"quaternion": {
"W": -0.7071067811865475,
"X": -0.0,
"Y": 0.0,
"Z": 0.7071067811865476
}
}
}
},
{
"ID": 7,
"pose": {
"translation": {
"x": -0.038099999999999995,
"y": 5.547867999999999,
"z": 1.4511020000000001
},
"rotation": {
"quaternion": {
"W": 1.0,
"X": 0.0,
"Y": 0.0,
"Z": 0.0
}
}
}
},
{
"ID": 8,
"pose": {
"translation": {
"x": -0.038099999999999995,
"y": 4.982717999999999,
"z": 1.4511020000000001
},
"rotation": {
"quaternion": {
"W": 1.0,
"X": 0.0,
"Y": 0.0,
"Z": 0.0
}
}
}
},
{
"ID": 9,
"pose": {
"translation": {
"x": 0.356108,
"y": 0.883666,
"z": 1.355852
},
"rotation": {
"quaternion": {
"W": 0.8660254037844387,
"X": 0.0,
"Y": 0.0,
"Z": 0.49999999999999994
}
}
}
},
{
"ID": 10,
"pose": {
"translation": {
"x": 1.4615159999999998,
"y": 0.24587199999999998,
"z": 1.355852
},
"rotation": {
"quaternion": {
"W": 0.8660254037844387,
"X": 0.0,
"Y": 0.0,
"Z": 0.49999999999999994
}
}
}
},
{
"ID": 11,
"pose": {
"translation": {
"x": 11.904726,
"y": 3.7132259999999997,
"z": 1.3208
},
"rotation": {
"quaternion": {
"W": -0.8660254037844387,
"X": -0.0,
"Y": 0.0,
"Z": 0.49999999999999994
}
}
}
},
{
"ID": 12,
"pose": {
"translation": {
"x": 11.904726,
"y": 4.49834,
"z": 1.3208
},
"rotation": {
"quaternion": {
"W": 0.8660254037844387,
"X": 0.0,
"Y": 0.0,
"Z": 0.49999999999999994
}
}
}
},
{
"ID": 13,
"pose": {
"translation": {
"x": 11.220196,
"y": 4.105148,
"z": 1.3208
},
"rotation": {
"quaternion": {
"W": 6.123233995736766e-17,
"X": 0.0,
"Y": 0.0,
"Z": 1.0
}
}
}
},
{
"ID": 14,
"pose": {
"translation": {
"x": 5.320792,
"y": 4.105148,
"z": 1.3208
},
"rotation": {
"quaternion": {
"W": 1.0,
"X": 0.0,
"Y": 0.0,
"Z": 0.0
}
}
}
},
{
"ID": 15,
"pose": {
"translation": {
"x": 4.641342,
"y": 4.49834,
"z": 1.3208
},
"rotation": {
"quaternion": {
"W": 0.5000000000000001,
"X": 0.0,
"Y": 0.0,
"Z": 0.8660254037844386
}
}
}
},
{
"ID": 16,
"pose": {
"translation": {
"x": 4.641342,
"y": 3.7132259999999997,
"z": 1.3208
},
"rotation": {
"quaternion": {
"W": -0.4999999999999998,
"X": -0.0,
"Y": 0.0,
"Z": 0.8660254037844387
}
}
}
}
],
"field": {
"length": 16.541,
"width": 8.211
}
}

BIN
wpical/src/main/native/resources/altfieldvideo/long.avi Normal file
View File

Binary file not shown.

BIN
wpical/src/main/native/resources/altfieldvideo/short.avi Normal file
View File

Binary file not shown.

BIN
wpical/src/main/native/resources/fieldvideo/long.mp4 Normal file
View File

Binary file not shown.

BIN
wpical/src/main/native/resources/fieldvideo/short.mp4 Normal file
View File

Binary file not shown.

1204
wpical/src/main/native/resources/lifecam_1280p_10x10.vnl Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
wpical/src/main/native/resources/testcalibration.avi Normal file
View File

Binary file not shown.

BIN
wpical/src/main/native/resources/testcalibration.mp4 Normal file
View File

Binary file not shown.

BIN
wpical/src/main/native/resources/wpical-128.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 1.5 KiB

BIN
wpical/src/main/native/resources/wpical-16.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 222 B

BIN
wpical/src/main/native/resources/wpical-256.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 3.9 KiB

BIN
wpical/src/main/native/resources/wpical-32.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 417 B

BIN
wpical/src/main/native/resources/wpical-48.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 618 B

BIN
wpical/src/main/native/resources/wpical-512.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 13 KiB

BIN
wpical/src/main/native/resources/wpical-64.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 777 B

301
wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h vendored Normal file
View File

@@ -0,0 +1,301 @@
// -*- mode: C; c-basic-offset: 2 -*-
// Copyright 2011 Oblong Industries
// 2017 Dima Kogan <dima@secretsauce.net>
// License: GNU LGPL <http://www.gnu.org/licenses>.
#pragma once
#include <suitesparse/cholmod.h>
#include <stdbool.h>
typedef void (dogleg_callback_t)(const double* p,
double* x,
cholmod_sparse* Jt,
void* cookie);
typedef void (dogleg_callback_dense_t)(const double* p,
double* x,
double* J,
void* cookie);
// an operating point of the solver
typedef struct
{
// The pointers in this structure are all indices into a single "pool" array
// (see allocOperatingPoint()). I thus don't need to store the pointers at
// all, and can just index that one array directly, but that would break my
// ABI
double* p;
double* x;
double norm2_x;
union
{
cholmod_sparse* Jt;
double* J_dense; // row-first: grad0, grad1, grad2, ...
};
double* Jt_x;
// the cached update vectors. It's useful to cache these so that when a step is rejected, we can
// reuse these when we retry
double* updateCauchy;
union
{
cholmod_dense* updateGN_cholmoddense;
double* updateGN_dense;
};
double updateCauchy_lensq, updateGN_lensq; // update vector lengths
// whether the current update vectors are correct or not
int updateCauchy_valid, updateGN_valid;
int didStepToEdgeOfTrustRegion;
double* step_to_here;
double step_to_here_len_sq;
} dogleg_operatingPoint_t;
// The newer APIs ( dogleg_...2() ) take a dogleg_parameters2_t argument for
// their settings, while the older ones use a global set of parameters specified
// with dogleg_set_...(). This global-parameters approach doesn't work if we
// have multiple users of libdogleg in the same application
typedef struct
{
int max_iterations;
int dogleg_debug;
// it is cheap to reject a too-large trust region, so I start with something
// "large". The solver will quickly move down to something reasonable. Only the
// user really knows if this is "large" or not, so they should change this via
// the API
double trustregion0;
// These are probably OK to leave alone. Tweaking them can maybe result in
// slightly faster convergence
double trustregion_decrease_factor;
double trustregion_decrease_threshold;
double trustregion_increase_factor;
double trustregion_increase_threshold;
// The termination thresholds. Documented in the header
double Jt_x_threshold;
double update_threshold;
double trustregion_threshold;
} dogleg_parameters2_t;
// solver context. This has all the internal state of the solver
typedef struct
{
cholmod_common common;
union
{
dogleg_callback_t* f;
dogleg_callback_dense_t* f_dense;
};
void* cookie;
// between steps, beforeStep contains the operating point of the last step.
// afterStep is ONLY used while making the step. Externally, use beforeStep
// unless you really know what you're doing
dogleg_operatingPoint_t* beforeStep;
dogleg_operatingPoint_t* afterStep;
// The result of the last JtJ factorization performed. Note that JtJ is not
// necessarily factorized at every step, so this is NOT guaranteed to contain
// the factorization of the most recent JtJ
union
{
cholmod_factor* factorization;
// This is a factorization of JtJ, stored as a packed symmetric matrix
// returned by dpptrf('L', ...)
double* factorization_dense;
};
// Have I ever seen a singular JtJ? If so, I add this constant to the diagonal
// from that point on. This is a simple and fast way to deal with
// singularities. This constant starts at 0, and is increased every time a
// singular JtJ is encountered. This is suboptimal but works for me for now.
double lambda;
// Are we using sparse math (cholmod)?
int is_sparse;
int Nstate, Nmeasurements;
const dogleg_parameters2_t* parameters;
} dogleg_solverContext_t;
#ifdef __cplusplus
extern "C" {
#endif
// Fills in the given structure with the default parameter set
void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters);
void dogleg_setMaxIterations(int n);
void dogleg_setTrustregionUpdateParameters(double downFactor, double downThreshold,
double upFactor, double upThreshold);
// The argument is a bit-mapped integer. Should be a bit-field structure or
// enum, but for API backwards-compatibility, I keep this an integer.
//
// if(debug == 0 ): no diagnostic output
// if(debug & DOGLEG_DEBUG_VNLOG): output vnlog diagnostics to stdout
// if(debug & ~DOGLEG_DEBUG_VNLOG): output human-oriented diagnostics to stderr
#define DOGLEG_DEBUG_VNLOG (1 << 30)
void dogleg_setDebug(int debug);
// The following parameters reflect the scaling of the problem being solved, so
// the user is strongly encouraged to tweak these. The defaults are set
// semi-arbitrarily
// The size of the trust region at start. It is cheap to reject a too-large
// trust region, so this should be something "large". Say 10x the length of an
// "expected" step size
void dogleg_setInitialTrustregion(double t);
// termination thresholds. These really depend on the scaling of the input
// problem, so the user should set these appropriately
//
// Jt_x threshold:
// The function being minimized is E = norm2(x) where x is a function of the state p.
// dE/dp = 2*Jt*x where Jt is transpose(dx/dp).
// if( for every i fabs(Jt_x[i]) < JT_X_THRESHOLD )
// { we are done; }
//
// update threshold:
// if(for every i fabs(state_update[i]) < UPDATE_THRESHOLD)
// { we are done; }
//
// trust region threshold:
// if(trustregion < TRUSTREGION_THRESHOLD)
// { we are done; }
//
// to leave a particular threshold alone, use a value <= 0 here
void dogleg_setThresholds(double Jt_x, double update, double trustregion);
// The main optimization function. Initial estimate of the solution passed in p,
// final optimized solution returned in p. p has Nstate variables. There are
// Nmeas measurements, the jacobian matrix has NJnnz non-zero entries.
//
// The evaluation function is given in the callback f; this function is passed
// the given cookie
//
// If we want to get the full solver state when we're done, a non-NULL
// returnContext pointer can be given. If this is done then THE USER IS
// RESPONSIBLE FOR FREEING ITS MEMORY WITH dogleg_freeContext()
double dogleg_optimize(double* p, unsigned int Nstate,
unsigned int Nmeas, unsigned int NJnnz,
dogleg_callback_t* f, void* cookie,
dogleg_solverContext_t** returnContext);
double dogleg_optimize2(double* p, unsigned int Nstate,
unsigned int Nmeas, unsigned int NJnnz,
dogleg_callback_t* f, void* cookie,
const dogleg_parameters2_t* parameters,
dogleg_solverContext_t** returnContext);
// Main optimization function if we're using dense linear algebra. The arguments
// are very similar to the sparse version: dogleg_optimize()
double dogleg_optimize_dense(double* p, unsigned int Nstate,
unsigned int Nmeas,
dogleg_callback_dense_t* f, void* cookie,
dogleg_solverContext_t** returnContext);
double dogleg_optimize_dense2(double* p, unsigned int Nstate,
unsigned int Nmeas,
dogleg_callback_dense_t* f, void* cookie,
const dogleg_parameters2_t* parameters,
dogleg_solverContext_t** returnContext);
// Compute the cholesky decomposition of JtJ. This function is only exposed if
// you need to touch libdogleg internals via returnContext. Sometimes after
// computing an optimization you want to do stuff with the factorization of JtJ,
// and this call ensures that the factorization is available. Most people don't
// need this function. If the comment wasn't clear, you don't need this
// function.
void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx);
// Generates a plain text table that contains gradient checks. This table can be
// easily parsed with "vnlog" tools
void dogleg_testGradient(unsigned int var, const double* p0,
unsigned int Nstate, unsigned int Nmeas, unsigned int NJnnz,
dogleg_callback_t* f, void* cookie);
void dogleg_testGradient_dense(unsigned int var, const double* p0,
unsigned int Nstate, unsigned int Nmeas,
dogleg_callback_dense_t* f, void* cookie);
// If we want to get the full solver state when we're done optimizing, we can
// pass a non-NULL returnContext pointer to dogleg_optimize(). If we do this,
// then the user MUST call dogleg_freeContext() to deallocate the pointer when
// the USER is done
void dogleg_freeContext(dogleg_solverContext_t** ctx);
// Computes outlierness factors. This function is experimental, and subject to
// change.
bool dogleg_getOutliernessFactors( // output
double* factors, // Nfeatures factors
// output, input
double* scale, // if <0 then I recompute
// inputs
// if outliers are grouped into features, the feature size is
// stated here
int featureSize,
int Nfeatures,
int NoutlierFeatures, // how many outliers we already have
dogleg_operatingPoint_t* point,
dogleg_solverContext_t* ctx );
// This stuff is experimental, and subject to change.
struct dogleg_outliers_t
{
unsigned char marked : 1;
};
bool dogleg_markOutliers(// output, input
struct dogleg_outliers_t* markedOutliers,
double* scale, // if <0 then I recompute
// output, input
int* Noutliers, // number of outliers before and after this call
// input
double (getConfidence)(int i_feature_exclude),
// if outliers are grouped into features, the feature size is
// stated here
int featureSize,
int Nfeatures,
dogleg_operatingPoint_t* point,
dogleg_solverContext_t* ctx);
void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude),
double* scale, // if <0 then I recompute
// if outliers are grouped into features, the feature size
// is stated here
int featureSize,
int Nfeatures,
int Noutliers, // how many outliers we already have
dogleg_operatingPoint_t* point,
dogleg_solverContext_t* ctx);
double dogleg_getOutliernessTrace_newFeature_sparse(const double* JqueryFeature,
int istateActive,
int NstateActive,
int featureSize,
int NoutlierFeatures,
dogleg_operatingPoint_t* point,
dogleg_solverContext_t* ctx);
#ifdef __cplusplus
} // extern "C"
#endif

2541
wpical/src/main/native/thirdparty/libdogleg/src/dogleg.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

3590
wpical/src/main/native/thirdparty/mrcal/generated/minimath_generated.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

582
wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh vendored Normal file
View File

@@ -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 <math.h>
#include <string.h>
#include "strides.h"
template<int NGRAD, int NVEC> struct vec_withgrad_t;
template<int NGRAD>
struct val_withgrad_t
{
double x;
double j[NGRAD == 0 ? 1 : NGRAD];
val_withgrad_t(double _x = 0.0) : x(_x)
{
for(int i=0; i<NGRAD; i++) j[i] = 0.0;
}
val_withgrad_t<NGRAD> operator+( const val_withgrad_t<NGRAD>& b ) const
{
val_withgrad_t<NGRAD> y = *this;
y.x += b.x;
for(int i=0; i<NGRAD; i++)
y.j[i] += b.j[i];
return y;
}
val_withgrad_t<NGRAD> operator+( double b ) const
{
val_withgrad_t<NGRAD> y = *this;
y.x += b;
return y;
}
void operator+=( const val_withgrad_t<NGRAD>& b )
{
*this = (*this) + b;
}
val_withgrad_t<NGRAD> operator-( const val_withgrad_t<NGRAD>& b ) const
{
val_withgrad_t<NGRAD> y = *this;
y.x -= b.x;
for(int i=0; i<NGRAD; i++)
y.j[i] -= b.j[i];
return y;
}
val_withgrad_t<NGRAD> operator-( double b ) const
{
val_withgrad_t<NGRAD> y = *this;
y.x -= b;
return y;
}
void operator-=( const val_withgrad_t<NGRAD>& b )
{
*this = (*this) - b;
}
val_withgrad_t<NGRAD> operator-() const
{
return (*this) * (-1);
}
val_withgrad_t<NGRAD> operator*( const val_withgrad_t<NGRAD>& b ) const
{
val_withgrad_t<NGRAD> y;
y.x = x * b.x;
for(int i=0; i<NGRAD; i++)
y.j[i] = j[i]*b.x + x*b.j[i];
return y;
}
val_withgrad_t<NGRAD> operator*( double b ) const
{
val_withgrad_t<NGRAD> y;
y.x = x * b;
for(int i=0; i<NGRAD; i++)
y.j[i] = j[i]*b;
return y;
}
void operator*=( const val_withgrad_t<NGRAD>& b )
{
*this = (*this) * b;
}
void operator*=( const double b )
{
*this = (*this) * b;
}
val_withgrad_t<NGRAD> operator/( const val_withgrad_t<NGRAD>& b ) const
{
val_withgrad_t<NGRAD> y;
y.x = x / b.x;
for(int i=0; i<NGRAD; i++)
y.j[i] = (j[i]*b.x - x*b.j[i]) / (b.x*b.x);
return y;
}
val_withgrad_t<NGRAD> operator/( double b ) const
{
return (*this) * (1./b);
}
void operator/=( const val_withgrad_t<NGRAD>& b )
{
*this = (*this) / b;
}
void operator/=( const double b )
{
*this = (*this) / b;
}
val_withgrad_t<NGRAD> sqrt(void) const
{
val_withgrad_t<NGRAD> y;
y.x = ::sqrt(x);
for(int i=0; i<NGRAD; i++)
y.j[i] = j[i] / (2. * y.x);
return y;
}
val_withgrad_t<NGRAD> square(void) const
{
val_withgrad_t<NGRAD> s;
s.x = x*x;
for(int i=0; i<NGRAD; i++)
s.j[i] = 2. * x * j[i];
return s;
}
val_withgrad_t<NGRAD> sin(void) const
{
const double s = ::sin(x);
const double c = ::cos(x);
val_withgrad_t<NGRAD> y;
y.x = s;
for(int i=0; i<NGRAD; i++)
y.j[i] = c*j[i];
return y;
}
val_withgrad_t<NGRAD> cos(void) const
{
const double s = ::sin(x);
const double c = ::cos(x);
val_withgrad_t<NGRAD> y;
y.x = c;
for(int i=0; i<NGRAD; i++)
y.j[i] = -s*j[i];
return y;
}
vec_withgrad_t<NGRAD, 2> sincos(void) const
{
const double s = ::sin(x);
const double c = ::cos(x);
vec_withgrad_t<NGRAD, 2> sc;
sc.v[0].x = s;
sc.v[1].x = c;
for(int i=0; i<NGRAD; i++)
{
sc.v[0].j[i] = c*j[i];
sc.v[1].j[i] = -s*j[i];
}
return sc;
}
val_withgrad_t<NGRAD> tan(void) const
{
const double s = ::sin(x);
const double c = ::cos(x);
val_withgrad_t<NGRAD> y;
y.x = s/c;
for(int i=0; i<NGRAD; i++)
y.j[i] = j[i] / (c*c);
return y;
}
val_withgrad_t<NGRAD> atan2(val_withgrad_t<NGRAD>& x) const
{
val_withgrad_t<NGRAD> th;
const val_withgrad_t<NGRAD>& 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<NGRAD; i++)
th.j[i] = (y.j[i]*x.x - y.x*x.j[i]) / norm2;
return th;
}
// This function does NOT check for overflow. The gradient is infinite at
// the valid bounds of the input. So the caller has to do the right thing in
// those cases
val_withgrad_t<NGRAD> asin(void) const
{
val_withgrad_t<NGRAD> th;
th.x = ::asin(x);
double dasin_dx = 1. / ::sqrt( 1. - x*x );
for(int i=0; i<NGRAD; i++)
th.j[i] = dasin_dx * j[i];
return th;
}
// This function does NOT check for overflow. The gradient is infinite at
// the valid bounds of the input. So the caller has to do the right thing in
// those cases
val_withgrad_t<NGRAD> acos(void) const
{
val_withgrad_t<NGRAD> th;
th.x = ::acos(x);
double dacos_dx = -1. / ::sqrt( 1. - x*x );
for(int i=0; i<NGRAD; i++)
th.j[i] = dacos_dx * j[i];
return th;
}
val_withgrad_t<NGRAD> sinx_over_x(// To avoid recomputing it
const val_withgrad_t<NGRAD>& 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<NGRAD>(1.0);
return sinx / (*this);
}
};
template<int NGRAD, int NVEC>
struct vec_withgrad_t
{
val_withgrad_t<NGRAD> v[NVEC == 0 ? 1 : NVEC];
vec_withgrad_t() {}
void init_vars(const double* x_in, int ivar0, int Nvars, int i_gradvec0 = -1,
int stride = sizeof(double))
{
// Initializes vector entries ivar0..ivar0+Nvars-1 inclusive using the
// data in x_in[]. x_in[0] corresponds to vector entry ivar0. If
// i_gradvec0 >= 0 then vector ivar0 corresponds to gradient index
// i_gradvec0, with all subsequent entries being filled-in
// consecutively. It's very possible that NGRAD > Nvars. Initially the
// subset of the gradient array corresponding to variables
// i_gradvec0..i_gradvec0+Nvars-1 is an identity, with the rest being 0
memset((char*)&v[ivar0], 0, Nvars*sizeof(v[0]));
for(int i=ivar0; i<ivar0+Nvars; i++)
{
v[i].x = _P1(x_in,stride, i-ivar0);
if(i_gradvec0 >= 0)
v[i].j[i_gradvec0+i-ivar0] = 1.0;
}
}
vec_withgrad_t(const double* x_in, int i_gradvec0 = -1,
int stride = sizeof(double))
{
init_vars(x_in, 0, NVEC, i_gradvec0, stride);
}
val_withgrad_t<NGRAD>& operator[](int i)
{
return v[i];
}
const val_withgrad_t<NGRAD>& operator[](int i) const
{
return v[i];
}
void operator+=( const vec_withgrad_t<NGRAD,NVEC>& x )
{
(*this) = (*this) + x;
}
vec_withgrad_t<NGRAD,NVEC> operator+( const vec_withgrad_t<NGRAD,NVEC>& x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] + x.v[i];
return p;
}
void operator+=( const val_withgrad_t<NGRAD>& x )
{
(*this) = (*this) + x;
}
vec_withgrad_t<NGRAD,NVEC> operator+( const val_withgrad_t<NGRAD>& x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] + x;
return p;
}
void operator+=( double x )
{
(*this) = (*this) + x;
}
vec_withgrad_t<NGRAD,NVEC> operator+( double x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] + x;
return p;
}
void operator-=( const vec_withgrad_t<NGRAD,NVEC>& x )
{
(*this) = (*this) - x;
}
vec_withgrad_t<NGRAD,NVEC> operator-( const vec_withgrad_t<NGRAD,NVEC>& x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] - x.v[i];
return p;
}
void operator-=( const val_withgrad_t<NGRAD>& x )
{
(*this) = (*this) - x;
}
vec_withgrad_t<NGRAD,NVEC> operator-( const val_withgrad_t<NGRAD>& x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] - x;
return p;
}
void operator-=( double x )
{
(*this) = (*this) - x;
}
vec_withgrad_t<NGRAD,NVEC> operator-( double x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] - x;
return p;
}
void operator*=( const vec_withgrad_t<NGRAD,NVEC>& x )
{
(*this) = (*this) * x;
}
vec_withgrad_t<NGRAD,NVEC> operator*( const vec_withgrad_t<NGRAD,NVEC>& x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] * x.v[i];
return p;
}
void operator*=( const val_withgrad_t<NGRAD>& x )
{
(*this) = (*this) * x;
}
vec_withgrad_t<NGRAD,NVEC> operator*( const val_withgrad_t<NGRAD>& x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] * x;
return p;
}
void operator*=( double x )
{
(*this) = (*this) * x;
}
vec_withgrad_t<NGRAD,NVEC> operator*( double x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] * x;
return p;
}
void operator/=( const vec_withgrad_t<NGRAD,NVEC>& x )
{
(*this) = (*this) / x;
}
vec_withgrad_t<NGRAD,NVEC> operator/( const vec_withgrad_t<NGRAD,NVEC>& x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] / x.v[i];
return p;
}
void operator/=( const val_withgrad_t<NGRAD>& x )
{
(*this) = (*this) / x;
}
vec_withgrad_t<NGRAD,NVEC> operator/( const val_withgrad_t<NGRAD>& x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] / x;
return p;
}
void operator/=( double x )
{
(*this) = (*this) / x;
}
vec_withgrad_t<NGRAD,NVEC> operator/( double x ) const
{
vec_withgrad_t<NGRAD,NVEC> p;
for(int i=0; i<NVEC; i++)
p[i] = v[i] / x;
return p;
}
val_withgrad_t<NGRAD> dot( const vec_withgrad_t<NGRAD,NVEC>& x) const
{
val_withgrad_t<NGRAD> d; // initializes to 0
for(int i=0; i<NVEC; i++)
{
val_withgrad_t<NGRAD> e = x.v[i]*v[i];
d += e;
}
return d;
}
vec_withgrad_t<NGRAD,NVEC> cross( const vec_withgrad_t<NGRAD,NVEC>& x) const
{
vec_withgrad_t<NGRAD,NVEC> c;
c[0] = v[1]*x.v[2] - v[2]*x.v[1];
c[1] = v[2]*x.v[0] - v[0]*x.v[2];
c[2] = v[0]*x.v[1] - v[1]*x.v[0];
return c;
}
val_withgrad_t<NGRAD> norm2(void) const
{
return dot(*this);
}
val_withgrad_t<NGRAD> mag(void) const
{
val_withgrad_t<NGRAD> l2 = norm2();
return l2.sqrt();
}
void extract_value(double* out,
int stride = sizeof(double),
int ivar0 = 0, int Nvars = NVEC) const
{
for(int i=ivar0; i<ivar0+Nvars; i++)
_P1(out,stride, i-ivar0) = v[i].x;
}
void extract_grad(double* J,
int i_gradvec0, int N_gradout,
int ivar0,
int J_stride0, int J_stride1,
int Nvars = NVEC) const
{
for(int i=ivar0; i<ivar0+Nvars; i++)
for(int j=0; j<N_gradout; j++)
_P2(J,J_stride0,J_stride1, i-ivar0,j) = v[i].j[i_gradvec0+j];
}
};
template<int NGRAD>
static
vec_withgrad_t<NGRAD, 3>
cross( const vec_withgrad_t<NGRAD, 3>& a,
const vec_withgrad_t<NGRAD, 3>& b )
{
vec_withgrad_t<NGRAD, 3> 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<int NGRAD>
static
val_withgrad_t<NGRAD>
cross_norm2( const vec_withgrad_t<NGRAD, 3>& a,
const vec_withgrad_t<NGRAD, 3>& b )
{
vec_withgrad_t<NGRAD, 3> c = cross(a,b);
return c.norm2();
}
template<int NGRAD>
static
val_withgrad_t<NGRAD>
cross_mag( const vec_withgrad_t<NGRAD, 3>& a,
const vec_withgrad_t<NGRAD, 3>& b )
{
vec_withgrad_t<NGRAD, 3> c = cross(a,b);
return c.mag();
}

130
wpical/src/main/native/thirdparty/mrcal/include/basic-geometry.h vendored Normal file
View File

@@ -0,0 +1,130 @@
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
#pragma once
// A 2D point or vector
//
// The individual elements can be accessed via .x and .y OR the vector can be
// accessed as an .xy[] array:
//
// mrcal_point2_t p = f();
//
// Now p.x and p.xy[0] refer to the same value.
typedef union
{
struct
{
double x,y;
};
double xy[2];
} mrcal_point2_t;
// A 3D point or vector
//
// The individual elements can be accessed via .x and .y and .z OR the vector
// can be accessed as an .xyz[] array:
//
// mrcal_point3_t p = f();
//
// Now p.x and p.xy[0] refer to the same value.
typedef union
{
struct
{
double x,y,z;
};
double xyz[3];
} mrcal_point3_t;
// Unconstrained 6DOF pose containing a Rodrigues rotation and a translation
typedef struct
{
mrcal_point3_t r,t;
} mrcal_pose_t;
//////////// Easy convenience stuff
////// point2
static double mrcal_point2_inner(const mrcal_point2_t a, const mrcal_point2_t b)
{
return
a.x*b.x +
a.y*b.y;
}
static double mrcal_point2_norm2(const mrcal_point2_t a)
{
return mrcal_point2_inner(a,a);
}
#define mrcal_point2_mag(a) sqrt(norm2(a)) // macro to not require #include <math.h>
static mrcal_point2_t mrcal_point2_add(const mrcal_point2_t a, const mrcal_point2_t b)
{
return (mrcal_point2_t){ .x = a.x + b.x,
.y = a.y + b.y };
}
static mrcal_point2_t mrcal_point2_sub(const mrcal_point2_t a, const mrcal_point2_t b)
{
return (mrcal_point2_t){ .x = a.x - b.x,
.y = a.y - b.y };
}
static mrcal_point2_t mrcal_point2_scale(const mrcal_point2_t a, const double s)
{
return (mrcal_point2_t){ .x = a.x * s,
.y = a.y * s };
}
////// point3
static double mrcal_point3_inner(const mrcal_point3_t a, const mrcal_point3_t b)
{
return
a.x*b.x +
a.y*b.y +
a.z*b.z;
}
static double mrcal_point3_norm2(const mrcal_point3_t a)
{
return mrcal_point3_inner(a,a);
}
#define mrcal_point3_mag(a) sqrt(mrcal_point3_norm2(a)) // macro to not require #include <math.h>
static mrcal_point3_t mrcal_point3_add(const mrcal_point3_t a, const mrcal_point3_t b)
{
return (mrcal_point3_t){ .x = a.x + b.x,
.y = a.y + b.y,
.z = a.z + b.z };
}
static mrcal_point3_t mrcal_point3_sub(const mrcal_point3_t a, const mrcal_point3_t b)
{
return (mrcal_point3_t){ .x = a.x - b.x,
.y = a.y - b.y,
.z = a.z - b.z };
}
static mrcal_point3_t mrcal_point3_scale(const mrcal_point3_t a, const double s)
{
return (mrcal_point3_t){ .x = a.x * s,
.y = a.y * s,
.z = a.z * s };
}
static mrcal_point3_t mrcal_point3_cross(const mrcal_point3_t a, const mrcal_point3_t b)
{
return (mrcal_point3_t){ .x = a.y*b.z - a.z*b.y,
.y = a.z*b.x - a.x*b.z,
.z = a.x*b.y - a.y*b.x };
}

23
wpical/src/main/native/thirdparty/mrcal/include/cahvore.h vendored Normal file
View File

@@ -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 <stdbool.h>
#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);

1988
wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath-extra.h vendored Normal file
View File

File diff suppressed because it is too large Load Diff

820
wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath.h vendored Normal file
View File

@@ -0,0 +1,820 @@
#pragma once
#ifdef __cplusplus
#define restrict
#endif
#include "minimath_generated.h"
// In all the following computations I use the expression obtained before a
// maxima expand(). factorsum()-ing the result doesn't appear to do much. Doing
// an expand() greatly increases the number of multiplications, but reduces the
// number of additions. In the specific case of a 4x4 symmetric determinant,
// expand() changes (23,40) to (16,58) +,*. 18 more *, but 7 fewer +.
// Session:
/*
(%i13) display2d : false;
(%o13) false
(%i31) sym2 : matrix([m0,m1],
[m1,m2]);
(%o31) matrix([m0,m1],[m1,m2])
(%i32) sym3 : matrix([m0,m1,m2],
[m1,m3,m4],
[m2,m4,m5]);
(%o32) matrix([m0,m1,m2],[m1,m3,m4],[m2,m4,m5])
(%i33) sym4 : matrix([m0,m1,m2,m3],
[m1,m4,m5,m6],
[m2,m5,m7,m8],
[m3,m6,m8,m9]);
(%i99) sym5 : matrix([m0,m1,m2, m3, m4 ],
[m1,m5,m6, m7, m8 ],
[m2,m6,m9, m10,m11],
[m3,m7,m10,m12,m13],
[m4,m8,m11,m13,m14]);
(%o99) matrix([m0,m1,m2,m3,m4],[m1,m5,m6,m7,m8],[m2,m6,m9,m10,m11],
[m3,m7,m10,m12,m13],[m4,m8,m11,m13,m14])
(%i34) determinant(sym4);
(%o34) 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))
(%i35) expand(determinant(sym4));
(%o35) m0*m4*m7*m9-m1^2*m7*m9-m0*m5^2*m9+2*m1*m2*m5*m9-m2^2*m4*m9-m0*m4*m8^2
+m1^2*m8^2+2*m0*m5*m6*m8-2*m1*m2*m6*m8-2*m1*m3*m5*m8
+2*m2*m3*m4*m8-m0*m6^2*m7+2*m1*m3*m6*m7-m3^2*m4*m7+m2^2*m6^2
-2*m2*m3*m5*m6+m3^2*m5^2
*/
/* The session to compute the determinants appears above. The only postprocessing was to
replace m.^2 -> 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

124
wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h vendored Normal file
View File

@@ -0,0 +1,124 @@
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
#pragma once
// mrcal images. These are completely uninteresting, and don't do anything
// better that other image read/write APIS. If you have image libraries running,
// use those. If not, the ones defined here should be light and painless
// I support several image types:
// - "uint8": 8-bit grayscale
// - "uint16": 16-bit grayscale (using the system endian-ness)
// - "bgr": 24-bit BGR color
//
// Each type defines several functions in the MRCAL_IMAGE_DECLARE() macro:
//
// - mrcal_image_TYPE_t container image
// - mrcal_image_TYPE_at(mrcal_image_TYPE_t* image, int x, int y)
// - mrcal_image_TYPE_at_const(const mrcal_image_TYPE_t* image, int x, int y)
// - mrcal_image_TYPE_t mrcal_image_TYPE_crop(mrcal_image_TYPE_t* image, in x0, int y0, int w, int h)
// - mrcal_image_TYPE_save (const char* filename, const mrcal_image_TYPE_t* image);
// - mrcal_image_TYPE_load( mrcal_image_TYPE_t* image, const char* filename);
//
// The image-loading functions require a few notes:
//
// An image structure to fill in is given. image->data will be allocated to the
// proper size. It is the caller's responsibility to free(image->data) when
// they're done. Usage sample:
//
// mrcal_image_uint8_t image;
// mrcal_image_uint8_load(&image, image_filename);
// .... do stuff ...
// free(image.data);
//
// mrcal_image_uint8_load() converts images to 8-bpp grayscale. Color and
// palettized images are accepted
//
// mrcal_image_uint16_load() does NOT convert images. The images being read must
// already be stored as 16bpp grayscale images
//
// mrcal_image_bgr_load() converts images to 24-bpp color
#include <stdint.h>
#include <stdbool.h>
typedef struct { uint8_t bgr[3]; } mrcal_bgr_t;
#define MRCAL_IMAGE_DECLARE(T, Tname) \
typedef struct \
{ \
union \
{ \
/* in pixels */ \
struct {int w, h;}; \
struct {int width, height;}; \
struct {int cols, rows;}; \
}; \
int stride; /* in bytes */ \
T* data; \
} mrcal_image_ ## Tname ## _t; \
\
static inline \
T* mrcal_image_ ## Tname ## _at(mrcal_image_ ## Tname ## _t* image, int x, int y) \
{ \
return &image->data[x + y*image->stride / sizeof(T)]; \
} \
\
static inline \
const T* mrcal_image_ ## Tname ## _at_const(const mrcal_image_ ## Tname ## _t* image, int x, int y) \
{ \
return &image->data[x + y*image->stride / sizeof(T)]; \
} \
\
static inline \
mrcal_image_ ## Tname ## _t \
mrcal_image_ ## Tname ## _crop(mrcal_image_ ## Tname ## _t* image, \
int x0, int y0, \
int w, int h) \
{ \
return (mrcal_image_ ## Tname ## _t){ .w = w, \
.h = h, \
.stride = image->stride, \
.data = mrcal_image_ ## Tname ## _at(image,x0,y0) }; \
}
#define MRCAL_IMAGE_SAVE_LOAD_DECLARE(T, Tname) \
bool mrcal_image_ ## Tname ## _save (const char* filename, const mrcal_image_ ## Tname ## _t* image); \
bool mrcal_image_ ## Tname ## _load( mrcal_image_ ## Tname ## _t* image, const char* filename);
// Common images types
MRCAL_IMAGE_DECLARE(uint8_t, uint8);
MRCAL_IMAGE_DECLARE(uint16_t, uint16);
MRCAL_IMAGE_DECLARE(mrcal_bgr_t, bgr);
MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint8_t, uint8);
MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint16_t, uint16);
MRCAL_IMAGE_SAVE_LOAD_DECLARE(mrcal_bgr_t, bgr);
// Uncommon types. Not everything supports these
MRCAL_IMAGE_DECLARE(int8_t, int8);
MRCAL_IMAGE_DECLARE(int16_t, int16);
MRCAL_IMAGE_DECLARE(int32_t, int32);
MRCAL_IMAGE_DECLARE(uint32_t, uint32);
MRCAL_IMAGE_DECLARE(int64_t, int64);
MRCAL_IMAGE_DECLARE(uint64_t, uint64);
MRCAL_IMAGE_DECLARE(float, float);
MRCAL_IMAGE_DECLARE(double, double);
// Load the image into whatever type is stored on disk
bool mrcal_image_anytype_load(// output
// This is ONE of the known types
mrcal_image_uint8_t* image,
int* bits_per_pixel,
int* channels,
// input
const char* filename);

109
wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h vendored Normal file
View File

@@ -0,0 +1,109 @@
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
#pragma once
// THESE ARE NOT A PART OF THE EXTERNAL API. Exported for the mrcal python
// wrapper only
// These models have no precomputed data
typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__precomputed_t;
typedef struct { int dummy; } mrcal_LENSMODEL_CAHVORE__precomputed_t;
// The splined stereographic models configuration parameters can be used to
// compute the segment size. I cache this computation
typedef struct
{
// The distance between adjacent knots (1 segment) is u_per_segment =
// 1/segments_per_u
double segments_per_u;
} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t;
typedef struct
{
bool ready;
union
{
#define PRECOMPUTED_STRUCT(s,n) mrcal_ ##s##__precomputed_t s##__precomputed;
MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT)
#undef PRECOMPUTED_STRUCT
};
} mrcal_projection_precomputed_t;
void _mrcal_project_internal_opencv( // outputs
mrcal_point2_t* q,
mrcal_point3_t* dq_dp, // may be NULL
double* dq_dintrinsics_nocore, // may be NULL
// inputs
const mrcal_point3_t* p,
int N,
const double* intrinsics,
int Nintrinsics);
bool _mrcal_project_internal( // out
mrcal_point2_t* q,
// Stored as a row-first array of shape (N,2,3). Each
// trailing ,3 dimension element is a mrcal_point3_t
mrcal_point3_t* dq_dp,
// core, distortions concatenated. Stored as a row-first
// array of shape (N,2,Nintrinsics). This is a DENSE array.
// High-parameter-count lens models have very sparse
// gradients here, and the internal project() function
// returns those sparsely. For now THIS function densifies
// all of these
double* dq_dintrinsics,
// in
const mrcal_point3_t* p,
int N,
const mrcal_lensmodel_t* lensmodel,
// core, distortions concatenated
const double* intrinsics,
int Nintrinsics,
const mrcal_projection_precomputed_t* precomputed);
void _mrcal_precompute_lensmodel_data(mrcal_projection_precomputed_t* precomputed,
const mrcal_lensmodel_t* lensmodel);
bool _mrcal_unproject_internal( // out
mrcal_point3_t* out,
// in
const mrcal_point2_t* q,
int N,
const mrcal_lensmodel_t* lensmodel,
// core, distortions concatenated
const double* intrinsics,
const mrcal_projection_precomputed_t* precomputed);
// Report the number of non-zero entries in the optimization jacobian
int _mrcal_num_j_nonzero(int Nobservations_board,
int Nobservations_point,
// May be NULL if we don't have any of these
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated,
int calibration_object_width_n,
int calibration_object_height_n,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed,
const mrcal_observation_board_t* observations_board,
const mrcal_observation_point_t* observations_point,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);

396
wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h vendored Normal file
View File

@@ -0,0 +1,396 @@
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
#pragma once
#include <stdint.h>
#include <stdbool.h>
#include "basic-geometry.h"
////////////////////////////////////////////////////////////////////////////////
//////////////////// Lens models
////////////////////////////////////////////////////////////////////////////////
// These are an "X macro": https://en.wikipedia.org/wiki/X_Macro
//
// The supported lens models and their parameter counts. Models with a
// configuration may have a dynamic parameter count; this is indicated here with
// a <0 value. These models report their parameter counts in the
// LENSMODEL_XXX__lensmodel_num_params() function, called by
// mrcal_lensmodel_num_params().
#define MRCAL_LENSMODEL_NOCONFIG_LIST(_) \
_(LENSMODEL_PINHOLE, 4) \
_(LENSMODEL_STEREOGRAPHIC, 4) /* Simple stereographic-only model */ \
_(LENSMODEL_LONLAT, 4) \
_(LENSMODEL_LATLON, 4) \
_(LENSMODEL_OPENCV4, 8) \
_(LENSMODEL_OPENCV5, 9) \
_(LENSMODEL_OPENCV8, 12) \
_(LENSMODEL_OPENCV12, 16) /* available in OpenCV >= 3.0.0) */ \
_(LENSMODEL_CAHVOR, 9)
#define MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(_) \
_(LENSMODEL_CAHVORE, 12)
#define MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(_) \
_(LENSMODEL_SPLINED_STEREOGRAPHIC, -1)
#define MRCAL_LENSMODEL_LIST(_) \
MRCAL_LENSMODEL_NOCONFIG_LIST(_) \
MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(_) \
MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(_)
// parametric models have no extra configuration
typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__config_t;
typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__config_t;
typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__config_t;
typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__config_t;
typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__config_t;
typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__config_t;
typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__config_t;
typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__config_t;
typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__config_t;
#define _MRCAL_ITEM_DEFINE_ELEMENT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) type name bitfield;
// Configuration for CAHVORE. These are given as an an
// "X macro": https://en.wikipedia.org/wiki/X_Macro
#define MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_, cookie) \
_(linearity, double, "d", ".2f", "lf", , cookie)
typedef struct
{
MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, )
} mrcal_LENSMODEL_CAHVORE__config_t;
// Configuration for the splined stereographic models. These are given as an an
// "X macro": https://en.wikipedia.org/wiki/X_Macro
#define MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(_, cookie) \
/* Maximum degree of each 1D polynomial. This is almost certainly 2 */ \
/* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */ \
_(order, uint16_t, "H", PRIu16,SCNu16, , cookie) \
/* We have a Nx by Ny grid of control points */ \
_(Nx, uint16_t, "H", PRIu16,SCNu16, , cookie) \
_(Ny, uint16_t, "H", PRIu16,SCNu16, , cookie) \
/* The horizontal field of view. Not including fov_y. It's proportional with */ \
/* Ny and Nx */ \
_(fov_x_deg, uint16_t, "H", PRIu16,SCNu16, , cookie)
typedef struct
{
MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, )
} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t;
// An X-macro-generated enum mrcal_lensmodel_type_t. This has an element for
// each entry in MRCAL_LENSMODEL_LIST (with "MRCAL_" prepended). This lensmodel
// type selects the lens model, but does NOT provide the configuration.
// mrcal_lensmodel_t does that.
#define _LIST_WITH_COMMA(s,n) ,MRCAL_ ## s
typedef enum
{ // Generic error. Rarely used in current mrcal
MRCAL_LENSMODEL_INVALID = -2,
// Configuration parsing failed
MRCAL_LENSMODEL_INVALID_BADCONFIG = -1,
// A configuration was required, but was missing entirely
MRCAL_LENSMODEL_INVALID_MISSINGCONFIG = -3,
// The model type wasn't known
MRCAL_LENSMODEL_INVALID_TYPE = -4,
// Dummy entry. Must be -1 so that successive values start at 0
_MRCAL_LENSMODEL_DUMMY = -1
// The rest, starting with 0
MRCAL_LENSMODEL_LIST( _LIST_WITH_COMMA ) } mrcal_lensmodel_type_t;
#undef _LIST_WITH_COMMA
// Defines a lens model: the type AND the configuration values
typedef struct
{
// The type of lensmodel. This is an enum, selecting elements of
// MRCAL_LENSMODEL_LIST (with "MRCAL_" prepended)
mrcal_lensmodel_type_t type;
// A union of all the possible configuration structures. We pick the
// structure type based on the value of "type
union
{
#define CONFIG_STRUCT(s,n) mrcal_ ##s##__config_t s##__config;
MRCAL_LENSMODEL_LIST(CONFIG_STRUCT)
#undef CONFIG_STRUCT
};
} mrcal_lensmodel_t;
typedef union
{
struct
{
double x2, y2;
};
double values[2];
} mrcal_calobject_warp_t;
#define MRCAL_NSTATE_CALOBJECT_WARP ((int)((sizeof(mrcal_calobject_warp_t)/sizeof(double))))
//// ADD CHANGES TO THE DOCS IN lensmodels.org
////
// An X-macro-generated mrcal_lensmodel_metadata_t. Each lens model type has
// some metadata that describes its inherent properties. These properties can be
// queried by calling mrcal_lensmodel_metadata() in C and
// mrcal.lensmodel_metadata_and_config() in Python. The available properties and
// their meaning are listed in MRCAL_LENSMODEL_META_LIST()
#define MRCAL_LENSMODEL_META_LIST(_, cookie) \
/* If true, this model contains an "intrinsics core". This is described */ \
/* in mrcal_intrinsics_core_t. If present, the 4 core parameters ALWAYS */ \
/* appear at the start of a model's parameter vector */ \
_(has_core, bool, "i",,, :1, cookie) \
\
/* Whether a model is able to project points behind the camera */ \
/* (z<0 in the camera coordinate system). Models based on a pinhole */ \
/* projection (pinhole, OpenCV, CAHVOR(E)) cannot do this. models based */ \
/* on a stereographic projection (stereographic, splined stereographic) */ \
/* can */ \
_(can_project_behind_camera, bool, "i",,, :1, cookie) \
\
/* Whether gradients are available for this model. Currently only */ \
/* CAHVORE does not have gradients */ \
_(has_gradients, bool, "i",,, :1, cookie) \
\
/* Whether this is a noncentral model.Currently the only noncentral */ \
/* model we have is CAHVORE. There will be more later. */ \
_(noncentral, bool, "i",,, :1, cookie)
typedef struct
{
MRCAL_LENSMODEL_META_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, )
} mrcal_lensmodel_metadata_t;
////////////////////////////////////////////////////////////////////////////////
//////////////////// Optimization
////////////////////////////////////////////////////////////////////////////////
// Used to specify which camera is making an observation. The "intrinsics" index
// is used to identify a specific camera, while the "extrinsics" index is used
// to locate a camera in space. If I have a camera that is moving over time, the
// intrinsics index will remain the same, while the extrinsics index will change
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: there should be a pool of these, and I should be indexing that pool"
#endif
typedef struct
{
// indexes the intrinsics array
int intrinsics;
// indexes the extrinsics array. -1 means "at coordinate system reference"
int extrinsics;
} mrcal_camera_index_t;
// An observation of a calibration board. Each "observation" is ONE camera
// observing a board
typedef struct
{
// which camera is making this observation
mrcal_camera_index_t icam;
// indexes the "frames" array to select the pose of the calibration object
// being observed
int iframe;
} mrcal_observation_board_t;
// An observation of a discrete point. Each "observation" is ONE camera
// observing a single point in space
typedef struct
{
// which camera is making this observation
mrcal_camera_index_t icam;
// indexes the "points" array to select the position of the point being
// observed
int i_point;
} mrcal_observation_point_t;
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: triangulated points into a pool. maybe allowing the intrinsics to move in the process"
#endif
// An observation of a discrete point where the point itself is NOT a part of
// the optimization, but computed implicitly via triangulation. This structure
// is very similar to mrcal_observation_point_t, except instead of i_point
// identifying the point being observed we have
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: FINISH DOC"
#endif
typedef struct
{
// which camera is making this observation
mrcal_camera_index_t icam;
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: DOCUMENT. CAN THIS BIT FIELD BE PACKED NICELY?"
#endif
// Set if this is the last camera observing this point. Any set of N>=2
// cameras can observe any point. All observations of a given point are
// stored consecutively, the last one being noted by this bit
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: do I really need this? I cannot look at the next observation to determine when this one is done?"
#endif
bool last_in_set : 1;
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: this is temporary. Should be a weight in observations_point_pool like all the other observations"
#endif
bool outlier : 1;
// Observed pixel coordinates. This works just like elements of
// observations_board_pool and observations_point_pool
mrcal_point3_t px;
} mrcal_observation_point_triangulated_t;
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: need a function to identify a vanilla calibration problem. It needs to not include any triangulated points. The noise propagation is different"
#endif
// Bits indicating which parts of the optimization problem being solved. We can
// ask mrcal to solve for ALL the lens parameters and ALL the geometry and
// everything else. OR we can ask mrcal to lock down some part of the
// optimization problem, and to solve for the rest. If any variables are locked
// down, we use their initial values passed-in to mrcal_optimize()
typedef struct
{
// If true, we solve for the intrinsics core. Applies only to those models
// that HAVE a core (fx,fy,cx,cy)
bool do_optimize_intrinsics_core : 1;
// If true, solve for the non-core lens parameters
bool do_optimize_intrinsics_distortions : 1;
// If true, solve for the geometry of the cameras
bool do_optimize_extrinsics : 1;
// If true, solve for the poses of the calibration object
bool do_optimize_frames : 1;
// If true, optimize the shape of the calibration object
bool do_optimize_calobject_warp : 1;
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: Need finer-grained regularization flags"
// #warning "triangulated-solve: Regularization flags should reflect do_optimize stuff and Ncameras stuff"
#endif
// If true, apply the regularization terms in the solver
bool do_apply_regularization : 1;
// Whether to try to find NEW outliers. The outliers given on
// input are respected regardless
bool do_apply_outlier_rejection : 1;
// Pull the distance between the first two cameras to 1.0
bool do_apply_regularization_unity_cam01: 1;
} mrcal_problem_selections_t;
// Constants used in a mrcal optimization. This is similar to
// mrcal_problem_selections_t, but contains numerical values rather than just
// bits
typedef struct
{
// The minimum distance of an observed discrete point from its observing
// camera. Any observation of a point below this range will be penalized to
// encourage the optimizer to move the point further away from the camera
double point_min_range;
// The maximum distance of an observed discrete point from its observing
// camera. Any observation of a point abive this range will be penalized to
// encourage the optimizer to move the point closer to the camera
double point_max_range;
} mrcal_problem_constants_t;
// An X-macro-generated mrcal_stats_t. This structure is returned by the
// optimizer, and contains some statistics about the optimization
#define MRCAL_STATS_ITEM(_) \
/* The RMS error of the optimized fit at the optimum. Generally the residual */ \
/* vector x contains error values for each element of q, so N observed pixels */ \
/* produce 2N measurements: len(x) = 2*N. And the RMS error is */ \
/* sqrt( norm2(x) / N ) */ \
_(double, rms_reproj_error__pixels, PyFloat_FromDouble) \
\
/* How many pixel observations were thrown out as outliers. Each pixel */ \
/* observation produces two measurements. Note that this INCLUDES any */ \
/* outliers that were passed-in at the start */ \
_(int, Noutliers_board, PyLong_FromLong) \
\
/* How many pixel observations were thrown out as outliers. Each pixel */ \
/* observation produces two measurements. Note that this INCLUDES any */ \
/* outliers that were passed-in at the start */ \
_(int, Noutliers_triangulated_point, PyLong_FromLong)
#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
// #warning "triangulated-solve: implement stats.Noutliers_triangulated_point; add to c-api.org"
#endif
#define MRCAL_STATS_ITEM_DEFINE(type, name, pyconverter) type name;
typedef struct
{
MRCAL_STATS_ITEM(MRCAL_STATS_ITEM_DEFINE)
} mrcal_stats_t;
////////////////////////////////////////////////////////////////////////////////
//////////////////// Layout of the measurement and state vectors
////////////////////////////////////////////////////////////////////////////////
// The "intrinsics core" of a camera. This defines the final step of a
// projection operation. For instance with a pinhole model we have
//
// q[0] = focal_xy[0] * x/z + center_xy[0]
// q[1] = focal_xy[1] * y/z + center_xy[1]
typedef struct
{
double focal_xy [2];
double center_xy[2];
} mrcal_intrinsics_core_t;
// structure containing a camera pose + lens model. Used for .cameramodel
// input/output
#define MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS \
double rt_cam_ref[6]; \
unsigned int imagersize[2]; \
mrcal_lensmodel_t lensmodel \
typedef struct
{
MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS;
// mrcal_cameramodel_t works for all lensmodels, so its intrinsics count is
// not known at compile time. mrcal_cameramodel_t is thus usable only as
// part of a larger structure or as a mrcal_cameramodel_t* argument to
// functions. To allocate new mrcal_cameramodel_t objects, use
// mrcal_cameramodel_LENSMODEL_XXX_t or malloc() with the proper intrinsics
// size taken into account
double intrinsics[0];
} mrcal_cameramodel_t;
#define DEFINE_mrcal_cameramodel_MODEL_t(s,n) \
typedef union \
{ \
mrcal_cameramodel_t m; \
struct \
{ \
MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS; \
double intrinsics[n]; \
}; \
} mrcal_cameramodel_ ## s ## _t;
MRCAL_LENSMODEL_NOCONFIG_LIST( DEFINE_mrcal_cameramodel_MODEL_t)
MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(DEFINE_mrcal_cameramodel_MODEL_t)

924
wpical/src/main/native/thirdparty/mrcal/include/mrcal.h vendored Normal file
View File

@@ -0,0 +1,924 @@
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
#pragma once
#include <stdbool.h>
#include <stdint.h>
#include "mrcal-types.h"
#include "poseutils.h"
#include "stereo.h"
#include "triangulation.h"
#include "mrcal-image.h"
////////////////////////////////////////////////////////////////////////////////
//////////////////// Lens models
////////////////////////////////////////////////////////////////////////////////
#ifdef __cplusplus
extern "C" {
#endif
// Return an array of strings listing all the available lens models
//
// These are all "unconfigured" strings that use "..." placeholders for any
// configuration values. Each returned string is a \0-terminated const char*. The
// end of the list is signified by a NULL pointer
const char* const* mrcal_supported_lensmodel_names( void ); // NULL-terminated array of char* strings
// Return true if the given mrcal_lensmodel_type_t specifies a valid lens model
static bool mrcal_lensmodel_type_is_valid(mrcal_lensmodel_type_t t)
{
return t >= 0;
}
// Evaluates to true if the given lens model is one of the supported OpenCV
// types
#define MRCAL_LENSMODEL_IS_OPENCV(d) (MRCAL_LENSMODEL_OPENCV4 <= (d) && (d) <= MRCAL_LENSMODEL_OPENCV12)
// Return a string describing a lens model.
//
// This function returns a static string. For models with no configuration, this
// is the FULL string for that model. For models with a configuration, the
// configuration values have "..." placeholders. These placeholders mean that
// the resulting strings do not define a lens model fully, and cannot be
// converted to a mrcal_lensmodel_t with mrcal_lensmodel_from_name()
//
// This is the inverse of mrcal_lensmodel_type_from_name()
const char* mrcal_lensmodel_name_unconfigured( const mrcal_lensmodel_t* lensmodel );
// Return a CONFIGURED string describing a lens model.
//
// This function generates a fully-configured string describing the given lens
// model. For models with no configuration, this is just the static string
// returned by mrcal_lensmodel_name_unconfigured(). For models that have a
// configuration, however, the configuration values are filled-in. The resulting
// string may be converted back into a mrcal_lensmodel_t by calling
// mrcal_lensmodel_from_name().
//
// This function writes the string into the given buffer "out". The size of the
// buffer is passed in the "size" argument. The meaning of "size" is as with
// snprintf(), which is used internally. Returns true on success
//
// This is the inverse of mrcal_lensmodel_from_name()
bool mrcal_lensmodel_name( char* out, int size,
const mrcal_lensmodel_t* lensmodel );
// Parse the lens model type from a lens model name string
//
// The configuration is ignored. Thus this function works even if the
// configuration is missing or unparseable. Unknown model names return
// MRCAL_LENSMODEL_INVALID_TYPE
//
// This is the inverse of mrcal_lensmodel_name_unconfigured()
mrcal_lensmodel_type_t mrcal_lensmodel_type_from_name( const char* name );
// Parse the full configured lens model from a lens model name string
//
// The lens mode type AND the configuration are read into a mrcal_lensmodel_t
// structure, which this function returns.
//
// On error returns false with lensmodel->type set to MRCAL_LENSMODEL_INVALID_...
//
// This is the inverse of mrcal_lensmodel_name()
bool mrcal_lensmodel_from_name( // output
mrcal_lensmodel_t* lensmodel,
// input
const char* name );
// Return a structure containing a model's metadata
//
// The available metadata is described in the definition of the
// MRCAL_LENSMODEL_META_LIST() macro
mrcal_lensmodel_metadata_t mrcal_lensmodel_metadata( const mrcal_lensmodel_t* lensmodel );
// Return the number of parameters required to specify a given lens model
//
// For models that have a configuration, the parameter count value generally
// depends on the configuration. For instance, splined models use the model
// parameters as the spline control points, so the spline density (specified in
// the configuration) directly affects how many parameters such a model requires
int mrcal_lensmodel_num_params( const mrcal_lensmodel_t* lensmodel );
// Return the locations of x and y spline knots
// Splined models are defined by the locations of their control points. These
// are arranged in a grid, the size and density of which is set by the model
// configuration. We fill-in the x knot locations into ux[] and the y locations
// into uy[]. ux[] and uy[] must be large-enough to hold configuration->Nx and
// configuration->Ny values respectively.
//
// This function applies to splined models only. Returns true on success
bool mrcal_knots_for_splined_models( double* ux, double* uy,
const mrcal_lensmodel_t* lensmodel);
////////////////////////////////////////////////////////////////////////////////
//////////////////// Projections
////////////////////////////////////////////////////////////////////////////////
// Project the given camera-coordinate-system points
//
// Compute a "projection", a mapping of points defined in the camera coordinate
// system to their observed pixel coordinates. If requested, gradients are
// computed as well.
//
// We project N 3D points p to N 2D pixel coordinates q using the given
// lensmodel and intrinsics parameter values.
//
// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
// ((N,2) mrcal_point3_t objects).
//
// if (dq_dintrinsics != NULL) we report the gradient dq/dintrinsics in a dense
// (N,2,Nintrinsics) array. Note that splined models have very high Nintrinsics
// and very sparse gradients. THIS function reports the gradients densely,
// however, so it is inefficient for splined models.
//
// This function supports CAHVORE distortions only if we don't ask for any
// gradients
//
// Projecting out-of-bounds points (beyond the field of view) returns undefined
// values. Generally things remain continuous even as we move off the imager
// domain. Pinhole-like projections will work normally if projecting a point
// behind the camera. Splined projections clamp to the nearest spline segment:
// the projection will fly off to infinity quickly since we're extrapolating a
// polynomial, but the function will remain continuous.
bool mrcal_project( // out
mrcal_point2_t* q,
mrcal_point3_t* dq_dp,
double* dq_dintrinsics,
// in
const mrcal_point3_t* p,
int N,
const mrcal_lensmodel_t* lensmodel,
// core, distortions concatenated
const double* intrinsics);
// Unproject the given pixel coordinates
//
// Compute an "unprojection", a mapping of pixel coordinates to the camera
// coordinate system.
//
// We unproject N 2D pixel coordinates q to N 3D direction vectors v using the
// given lensmodel and intrinsics parameter values. The returned vectors v are
// not normalized, and may have any length.
// This is the "reverse" direction, so an iterative nonlinear optimization is
// performed internally to compute this result. This is much slower than
// mrcal_project(). For OpenCV models specifically, OpenCV has
// cvUndistortPoints() (and cv2.undistortPoints()), but these are unreliable:
// https://github.com/opencv/opencv/issues/8811
//
// This function does NOT support CAHVORE
bool mrcal_unproject( // out
mrcal_point3_t* v,
// in
const mrcal_point2_t* q,
int N,
const mrcal_lensmodel_t* lensmodel,
// core, distortions concatenated
const double* intrinsics);
// Project the given camera-coordinate-system points using a pinhole
// model. See the docs for projection details:
// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole
//
// This is a simplified special case of mrcal_project(). We project N
// camera-coordinate-system points p to N pixel coordinates q
//
// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
// ((N,2) mrcal_point3_t objects).
void mrcal_project_pinhole( // output
mrcal_point2_t* q,
mrcal_point3_t* dq_dp,
// input
const mrcal_point3_t* p,
int N,
const double* fxycxy);
// Unproject the given pixel coordinates using a pinhole model.
// See the docs for projection details:
// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole
//
// This is a simplified special case of mrcal_unproject(). We unproject N 2D
// pixel coordinates q to N camera-coordinate-system vectors v. The returned
// vectors v are not normalized, and may have any length.
//
// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array
// ((N,3) mrcal_point2_t objects).
void mrcal_unproject_pinhole( // output
mrcal_point3_t* v,
mrcal_point2_t* dv_dq,
// input
const mrcal_point2_t* q,
int N,
const double* fxycxy);
// Project the given camera-coordinate-system points using a stereographic
// model. See the docs for projection details:
// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic
//
// This is a simplified special case of mrcal_project(). We project N
// camera-coordinate-system points p to N pixel coordinates q
//
// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
// ((N,2) mrcal_point3_t objects).
void mrcal_project_stereographic( // output
mrcal_point2_t* q,
mrcal_point3_t* dq_dp,
// input
const mrcal_point3_t* p,
int N,
const double* fxycxy);
// Unproject the given pixel coordinates using a stereographic model.
// See the docs for projection details:
// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic
//
// This is a simplified special case of mrcal_unproject(). We unproject N 2D
// pixel coordinates q to N camera-coordinate-system vectors v. The returned
// vectors v are not normalized, and may have any length.
//
// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array
// ((N,3) mrcal_point2_t objects).
void mrcal_unproject_stereographic( // output
mrcal_point3_t* v,
mrcal_point2_t* dv_dq,
// input
const mrcal_point2_t* q,
int N,
const double* fxycxy);
// Project the given camera-coordinate-system points using an equirectangular
// projection. See the docs for projection details:
// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat
//
// This is a simplified special case of mrcal_project(). We project N
// camera-coordinate-system points p to N pixel coordinates q
//
// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
// ((N,2) mrcal_point3_t objects).
void mrcal_project_lonlat( // output
mrcal_point2_t* q,
mrcal_point3_t* dq_dv, // May be NULL. Each point
// gets a block of 2 mrcal_point3_t
// objects
// input
const mrcal_point3_t* v,
int N,
const double* fxycxy);
// Unproject the given pixel coordinates using an equirectangular projection.
// See the docs for projection details:
// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat
//
// This is a simplified special case of mrcal_unproject(). We unproject N 2D
// pixel coordinates q to N camera-coordinate-system vectors v. The returned
// vectors v are normalized.
//
// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array
// ((N,3) mrcal_point2_t objects).
void mrcal_unproject_lonlat( // output
mrcal_point3_t* v,
mrcal_point2_t* dv_dq, // May be NULL. Each point
// gets a block of 3 mrcal_point2_t
// objects
// input
const mrcal_point2_t* q,
int N,
const double* fxycxy);
// Project the given camera-coordinate-system points using a transverse
// equirectangular projection. See the docs for projection details:
// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon
//
// This is a simplified special case of mrcal_project(). We project N
// camera-coordinate-system points p to N pixel coordinates q
//
// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
// ((N,2) mrcal_point3_t objects).
void mrcal_project_latlon( // output
mrcal_point2_t* q,
mrcal_point3_t* dq_dv, // May be NULL. Each point
// gets a block of 2 mrcal_point3_t
// objects
// input
const mrcal_point3_t* v,
int N,
const double* fxycxy);
// Unproject the given pixel coordinates using a transverse equirectangular
// projection. See the docs for projection details:
// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon
//
// This is a simplified special case of mrcal_unproject(). We unproject N 2D
// pixel coordinates q to N camera-coordinate-system vectors v. The returned
// vectors v are normalized.
//
// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array
// ((N,3) mrcal_point2_t objects).
void mrcal_unproject_latlon( // output
mrcal_point3_t* v,
mrcal_point2_t* dv_dq, // May be NULL. Each point
// gets a block of 3 mrcal_point2_t
// objects
// input
const mrcal_point2_t* q,
int N,
const double* fxycxy);
////////////////////////////////////////////////////////////////////////////////
//////////////////// Optimization
////////////////////////////////////////////////////////////////////////////////
// Return the number of parameters needed in optimizing the given lens model
//
// This is identical to mrcal_lensmodel_num_params(), but takes into account the
// problem selections. Any intrinsics parameters locked down in the
// mrcal_problem_selections_t do NOT count towards the optimization parameters
int mrcal_num_intrinsics_optimization_params( mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel );
// Scales a state vector to the packed, unitless form used by the optimizer
//
// In order to make the optimization well-behaved, we scale all the variables in
// the state and the gradients before passing them to the optimizer. The internal
// optimization library thus works only with unitless (or "packed") data.
//
// This function takes an (Nstate,) array of full-units values b[], and scales
// it to produce packed data. This function applies the scaling directly to the
// input array; the input is modified, and nothing is returned.
//
// This is the inverse of mrcal_unpack_solver_state_vector()
void mrcal_pack_solver_state_vector( // out, in
double* b,
// in
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
// Scales a state vector from the packed, unitless form used by the optimizer
//
// In order to make the optimization well-behaved, we scale all the variables in
// the state and the gradients before passing them to the optimizer. The internal
// optimization library thus works only with unitless (or "packed") data.
//
// This function takes an (Nstate,) array of unitless values b[], and scales it
// to produce full-units data. This function applies the scaling directly to the
// input array; the input is modified, and nothing is returned.
//
// This is the inverse of mrcal_pack_solver_state_vector()
void mrcal_unpack_solver_state_vector( // out, in
double* b, // unitless state on input,
// scaled, meaningful state on
// output
// in
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
// Reports the icam_extrinsics corresponding to a given icam_intrinsics.
//
// If we're solving a vanilla calibration problem (stationary cameras observing
// a moving calibration object), each camera has a unique intrinsics index and a
// unique extrinsics index. This function reports the latter, given the former.
//
// On success, the result is written to *icam_extrinsics, and we return true. If
// the given camera is at the reference coordinate system, it has no extrinsics,
// and we report -1.
//
// If we have moving cameras (NOT a vanilla calibration problem), there isn't a
// single icam_extrinsics for a given icam_intrinsics, and we report an error by
// returning false
bool mrcal_corresponding_icam_extrinsics(// out
int* icam_extrinsics,
// in
int icam_intrinsics,
int Ncameras_intrinsics,
int Ncameras_extrinsics,
int Nobservations_board,
const mrcal_observation_board_t* observations_board,
int Nobservations_point,
const mrcal_observation_point_t* observations_point);
// Solve the given optimization problem
//
// This is the entry point to the mrcal optimization routine. The argument list
// is commented.
mrcal_stats_t
mrcal_optimize( // out
// Each one of these output pointers may be NULL
// Shape (Nstate,)
double* b_packed,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_b_packed,
// Shape (Nmeasurements,)
double* x,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_x,
// out, in
// These are a seed on input, solution on output
// intrinsics is a concatenation of the intrinsics core and the
// distortion params. The specific distortion parameters may
// vary, depending on lensmodel, so this is a variable-length
// structure
double* intrinsics, // Ncameras_intrinsics * NlensParams
mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame
mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame
mrcal_point3_t* points, // Npoints of these. In the reference frame
mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp
// in
int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
int Npoints, int Npoints_fixed, // at the end of points[]
const mrcal_observation_board_t* observations_board,
const mrcal_observation_point_t* observations_point,
int Nobservations_board,
int Nobservations_point,
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated,
// All the board pixel observations, in an array of shape
//
// ( Nobservations_board,
// calibration_object_height_n,
// calibration_object_width_n )
//
// .x, .y are the pixel observations .z is the weight of the
// observation. Most of the weights are expected to be 1.0. Less
// precise observations have lower weights.
//
// .z<0 indicates that this is an outlier. This is respected on
// input (even if !do_apply_outlier_rejection). New outliers are
// marked with .z<0 on output, so this isn't const
mrcal_point3_t* observations_board_pool,
// Same this, but for discrete points. Array of shape
//
// ( Nobservations_point,)
mrcal_point3_t* observations_point_pool,
const mrcal_lensmodel_t* lensmodel,
const int* imagersizes, // Ncameras_intrinsics*2 of these
mrcal_problem_selections_t problem_selections,
const mrcal_problem_constants_t* problem_constants,
double calibration_object_spacing,
int calibration_object_width_n,
int calibration_object_height_n,
bool verbose,
bool check_gradient);
// These are cholmod_sparse, cholmod_factor, cholmod_common. I don't want to
// include the full header that defines these in mrcal.h, and I don't need to:
// mrcal.h just needs to know that these are a structure
struct cholmod_sparse_struct;
struct cholmod_factor_struct;
struct cholmod_common_struct;
// Evaluate the value of the callback function at the given operating point
//
// The main optimization routine in mrcal_optimize() searches for optimal
// parameters by repeatedly calling a function to evaluate each hypothethical
// parameter set. This evaluation function is available by itself here,
// separated from the optimization loop. The arguments are largely the same as
// those to mrcal_optimize(), but the inputs are all read-only It is expected
// that this will be called from Python only.
bool mrcal_optimizer_callback(// out
// These output pointers may NOT be NULL, unlike
// their analogues in mrcal_optimize()
// Shape (Nstate,)
double* b_packed,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_b_packed,
// Shape (Nmeasurements,)
double* x,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_x,
// output Jacobian. May be NULL if we don't need
// it. This is the unitless Jacobian, used by the
// internal optimization routines
struct cholmod_sparse_struct* Jt,
// in
// intrinsics is a concatenation of the intrinsics core
// and the distortion params. The specific distortion
// parameters may vary, depending on lensmodel, so
// this is a variable-length structure
const double* intrinsics, // Ncameras_intrinsics * NlensParams
const mrcal_pose_t* extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame
const mrcal_pose_t* frames_toref, // Nframes of these. Transform TO the reference frame
const mrcal_point3_t* points, // Npoints of these. In the reference frame
const mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp
int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
int Npoints, int Npoints_fixed, // at the end of points[]
const mrcal_observation_board_t* observations_board,
const mrcal_observation_point_t* observations_point,
int Nobservations_board,
int Nobservations_point,
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated,
// All the board pixel observations, in an array of shape
//
// ( Nobservations_board,
// calibration_object_height_n,
// calibration_object_width_n )
//
// .x, .y are the pixel observations .z is the
// weight of the observation. Most of the weights
// are expected to be 1.0. Less precise
// observations have lower weights.
//
// .z<0 indicates that this is an outlier
const mrcal_point3_t* observations_board_pool,
// Same this, but for discrete points. Array of shape
//
// ( Nobservations_point,)
const mrcal_point3_t* observations_point_pool,
const mrcal_lensmodel_t* lensmodel,
const int* imagersizes, // Ncameras_intrinsics*2 of these
mrcal_problem_selections_t problem_selections,
const mrcal_problem_constants_t* problem_constants,
double calibration_object_spacing,
int calibration_object_width_n,
int calibration_object_height_n,
bool verbose);
bool mrcal_drt_ref_refperturbed__dbpacked(// output
// Shape (6,Nstate_frames)
double* Kpackedf,
int Kpackedf_stride0, // in bytes. <= 0 means "contiguous"
int Kpackedf_stride1, // in bytes. <= 0 means "contiguous"
// Shape (6,Nstate_points)
double* Kpackedp,
int Kpackedp_stride0, // in bytes. <= 0 means "contiguous"
int Kpackedp_stride1, // in bytes. <= 0 means "contiguous"
// Shape (6,Nstate_calobject_warp)
double* Kpackedcw,
int Kpackedcw_stride0, // in bytes. <= 0 means "contiguous"
int Kpackedcw_stride1, // in bytes. <= 0 means "contiguous"
// inputs
// stuff that describes this solve
const double* b_packed,
// used only to confirm that the user passed-in the buffer they
// should have passed-in. The size must match exactly
int buffer_size_b_packed,
// The unitless (packed) Jacobian,
// used by the internal optimization
// routines cholmod_analyze() and
// cholmod_factorize() require
// non-const
/* const */
struct cholmod_sparse_struct* Jt,
// meta-parameters
int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
int Npoints, int Npoints_fixed, // at the end of points[]
int Nobservations_board,
int Nobservations_point,
const mrcal_lensmodel_t* lensmodel,
mrcal_problem_selections_t problem_selections,
int calibration_object_width_n,
int calibration_object_height_n);
////////////////////////////////////////////////////////////////////////////////
//////////////////// Layout of the measurement and state vectors
////////////////////////////////////////////////////////////////////////////////
// The optimization routine tries to minimize the length of the measurement
// vector x by moving around the state vector b.
//
// Depending on the specific optimization problem being solved and the
// mrcal_problem_selections_t, the state vector may contain any of
// - The lens parameters
// - The geometry of the cameras
// - The geometry of the observed chessboards and discrete points
// - The chessboard shape
//
// The measurement vector may contain
// - The errors in observations of the chessboards
// - The errors in observations of discrete points
// - The penalties in the solved point positions
// - The regularization terms
//
// Given the problem selections and a vector b or x it is often useful to know
// where specific quantities lie in those vectors. We have 4 sets of functions
// to answer such questions:
//
// int mrcal_measurement_index_THING()
// Returns the index in the measurement vector x where the contiguous block of
// values describing the THING begins. THING is any of
// - boards
// - points
// - regularization
//
// int mrcal_num_measurements_THING()
// Returns the number of values in the contiguous block in the measurement
// vector x that describe the given THING. THING is any of
// - boards
// - points
// - regularization
//
// int mrcal_state_index_THING()
// Returns the index in the state vector b where the contiguous block of
// values describing the THING begins. THING is any of
// - intrinsics
// - extrinsics
// - frames
// - points
// - calobject_warp
// If we're not optimizing the THING, return <0
//
// int mrcal_num_states_THING()
// Returns the number of values in the contiguous block in the state
// vector b that describe the given THING. THING is any of
// - intrinsics
// - extrinsics
// - frames
// - points
// - calobject_warp
// If we're not optimizing the THING, return 0
int mrcal_measurement_index_boards(int i_observation_board,
int Nobservations_board,
int Nobservations_point,
int calibration_object_width_n,
int calibration_object_height_n);
int mrcal_num_measurements_boards(int Nobservations_board,
int calibration_object_width_n,
int calibration_object_height_n);
int mrcal_measurement_index_points(int i_observation_point,
int Nobservations_board,
int Nobservations_point,
int calibration_object_width_n,
int calibration_object_height_n);
int mrcal_num_measurements_points(int Nobservations_point);
int mrcal_measurement_index_points_triangulated(int i_point_triangulated,
int Nobservations_board,
int Nobservations_point,
// May be NULL if we don't have any of these
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated,
int calibration_object_width_n,
int calibration_object_height_n);
int mrcal_num_measurements_points_triangulated(// May be NULL if we don't have any of these
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated);
int mrcal_num_measurements_points_triangulated_initial_Npoints(// May be NULL if we don't have any of these
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated,
// Only consider the leading Npoints. If Npoints < 0: take ALL the points
int Npoints);
bool mrcal_decode_observation_indices_points_triangulated(
// output
int* iobservation0,
int* iobservation1,
int* iobservation_point0,
int* Nobservations_this_point,
int* Nmeasurements_this_point,
int* ipoint,
// input
const int imeasurement,
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated);
int mrcal_measurement_index_regularization(// May be NULL if we don't have any of these
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated,
int calibration_object_width_n,
int calibration_object_height_n,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board, int Nobservations_point,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_num_measurements(int Nobservations_board,
int Nobservations_point,
// May be NULL if we don't have any of these
const mrcal_observation_point_triangulated_t* observations_point_triangulated,
int Nobservations_point_triangulated,
int calibration_object_width_n,
int calibration_object_height_n,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_num_states(int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_state_index_intrinsics(int icam_intrinsics,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_num_states_intrinsics(int Ncameras_intrinsics,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_state_index_extrinsics(int icam_extrinsics,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_num_states_extrinsics(int Ncameras_extrinsics,
mrcal_problem_selections_t problem_selections);
int mrcal_state_index_frames(int iframe,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_num_states_frames(int Nframes,
mrcal_problem_selections_t problem_selections);
int mrcal_state_index_points(int i_point,
int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_num_states_points(int Npoints, int Npoints_fixed,
mrcal_problem_selections_t problem_selections);
int mrcal_state_index_calobject_warp(int Ncameras_intrinsics, int Ncameras_extrinsics,
int Nframes,
int Npoints, int Npoints_fixed, int Nobservations_board,
mrcal_problem_selections_t problem_selections,
const mrcal_lensmodel_t* lensmodel);
int mrcal_num_states_calobject_warp(mrcal_problem_selections_t problem_selections,
int Nobservations_board);
// if len>0, the string doesn't need to be 0-terminated. If len<=0, the end of
// the buffer IS indicated by a '\0' byte
//
// return NULL on error
mrcal_cameramodel_t* mrcal_read_cameramodel_string(const char* string, int len);
mrcal_cameramodel_t* mrcal_read_cameramodel_file (const char* filename);
void mrcal_free_cameramodel(mrcal_cameramodel_t** cameramodel);
bool mrcal_write_cameramodel_file(const char* filename,
const mrcal_cameramodel_t* cameramodel);
#ifdef __cplusplus
} // extern "C"
#endif
#define DECLARE_mrcal_apply_color_map(T,Tname) \
bool mrcal_apply_color_map_##Tname( \
mrcal_image_bgr_t* out, \
const mrcal_image_##Tname##_t* in, \
\
/* If true, I set in_min/in_max from the */ \
/* min/max of the input data */ \
const bool auto_min, \
const bool auto_max, \
\
/* If true, I implement gnuplot's default 7,5,15 mapping. */ \
/* This is a reasonable default choice. */ \
/* function_red/green/blue are ignored if true */ \
const bool auto_function, \
\
/* min/max input values to use if not */ \
/* auto_min/auto_max */ \
T in_min, /* will map to 0 */ \
T in_max, /* will map to 255 */ \
\
/* The color mappings to use. If !auto_function */ \
int function_red, \
int function_green, \
int function_blue)
DECLARE_mrcal_apply_color_map(uint8_t, uint8);
DECLARE_mrcal_apply_color_map(uint16_t, uint16);
DECLARE_mrcal_apply_color_map(uint32_t, uint32);
DECLARE_mrcal_apply_color_map(uint64_t, uint64);
DECLARE_mrcal_apply_color_map(int8_t, int8);
DECLARE_mrcal_apply_color_map(int16_t, int16);
DECLARE_mrcal_apply_color_map(int32_t, int32);
DECLARE_mrcal_apply_color_map(int64_t, int64);
DECLARE_mrcal_apply_color_map(float, float);
DECLARE_mrcal_apply_color_map(double, double);
#undef DECLARE_mrcal_apply_color_map
// returns false on error
typedef bool (*mrcal_callback_sensor_link_t)(const uint16_t idx_to,
const uint16_t idx_from,
void* cookie);
// Traverses a connectivity graph of sensors to find the best connection from
// the root sensor (idx==0) to every other sensor. This is useful to seed a
// problem with sparse connections, where every sensor doesn't have overlapping
// observations with every other sensor. See the docstring for
// mrcal.traverse_sensor_links() for details (that Python function wraps
// this one). Note: this C function takes a packed connectivity matrix (just the
// upper triangle stored), while the Python function takes a full (N,N) array,
// while assuming it is symmetric and has a 0 diagonal
//
// returns false on error
bool mrcal_traverse_sensor_links( const uint16_t Nsensors,
// (N,N) symmetric matrix with a 0 diagonal.
// I store the upper triangle only,
// row-first: a 1D array of (N*(N-1)/2)
// values. use pairwise_index() to index
const uint16_t* connectivity_matrix,
const mrcal_callback_sensor_link_t cb,
void* cookie);
// Public ABI stuff, that's not for end-user consumption
#include "mrcal-internal.h"

586
wpical/src/main/native/thirdparty/mrcal/include/poseutils.h vendored Normal file
View File

@@ -0,0 +1,586 @@
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
#pragma once
#include <stdbool.h>
// Unless specified all arrays stored in contiguous matrices in row-major order.
//
// All functions are defined using the mrcal_..._full() form, which supports
// non-contiguous input and output arrays, and some optional arguments. Strides
// are used to specify the array layout.
//
// All functions have a convenience wrapper macro that is a simpler way to call
// the function, usable with contiguous arrays and defaults.
//
// All the functions use double-precision floating point to store the data, and
// C ints to store strides. The strides are given in bytes. In the
// mrcal_..._full() functions, each array is followed by the strides, one per
// dimension.
//
// I have two different representations of pose transformations:
//
// - Rt is a concatenated (4,3) array: Rt = nps.glue(R,t, axis=-2). The
// transformation is R*x+t
//
// - rt is a concatenated (6,) array: rt = nps.glue(r,t, axis=-1). The
// transformation is R*x+t where R = R_from_r(r)
//
// I treat all vectors as column vectors, so matrix multiplication works from
// the left: to rotate a vector x by a rotation matrix R I have
//
// x_rotated = R * x
// Store an identity rotation matrix into the given (3,3) array
//
// This is simply an identity matrix
#define mrcal_identity_R(R) mrcal_identity_R_full(R,0,0)
void mrcal_identity_R_full(double* R, // (3,3) array
int R_stride0, // in bytes. <= 0 means "contiguous"
int R_stride1 // in bytes. <= 0 means "contiguous"
);
// Store an identity rodrigues rotation into the given (3,) array
//
// This is simply an array of zeros
#define mrcal_identity_r(r) mrcal_identity_r_full(r,0)
void mrcal_identity_r_full(double* r, // (3,) array
int r_stride0 // in bytes. <= 0 means "contiguous"
);
// Store an identity Rt transformation into the given (4,3) array
#define mrcal_identity_Rt(Rt) mrcal_identity_Rt_full(Rt,0,0)
void mrcal_identity_Rt_full(double* Rt, // (4,3) array
int Rt_stride0, // in bytes. <= 0 means "contiguous"
int Rt_stride1 // in bytes. <= 0 means "contiguous"
);
// Store an identity rt transformation into the given (6,) array
#define mrcal_identity_rt(rt) mrcal_identity_rt_full(rt,0)
void mrcal_identity_rt_full(double* rt, // (6,) array
int rt_stride0 // in bytes. <= 0 means "contiguous"
);
// Rotate the point x_in in a (3,) array by the rotation matrix R in a (3,3)
// array. This is simply the matrix-vector multiplication R x_in
//
// The result is returned in a (3,) array x_out.
//
// The gradient dx_out/dR is returned in a (3, 3,3) array J_R. Set to NULL if
// this is not wanted
//
// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
// the matrix R. Set to NULL if this is not wanted
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_rotate_point_R( x_out,J_R,J_x,R,x_in) mrcal_rotate_point_R_full(x_out,0,J_R,0,0,0,J_x,0,0,R,0,0,x_in,0, false)
#define mrcal_rotate_point_R_inverted(x_out,J_R,J_x,R,x_in) mrcal_rotate_point_R_full(x_out,0,J_R,0,0,0,J_x,0,0,R,0,0,x_in,0, true)
void mrcal_rotate_point_R_full( // output
double* x_out, // (3,) array
int x_out_stride0, // in bytes. <= 0 means "contiguous"
double* J_R, // (3,3,3) array. May be NULL
int J_R_stride0, // in bytes. <= 0 means "contiguous"
int J_R_stride1, // in bytes. <= 0 means "contiguous"
int J_R_stride2, // in bytes. <= 0 means "contiguous"
double* J_x, // (3,3) array. May be NULL
int J_x_stride0, // in bytes. <= 0 means "contiguous"
int J_x_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* R, // (3,3) array. May be NULL
int R_stride0, // in bytes. <= 0 means "contiguous"
int R_stride1, // in bytes. <= 0 means "contiguous"
const double* x_in, // (3,) array. May be NULL
int x_in_stride0, // in bytes. <= 0 means "contiguous"
bool inverted // if true, I apply a
// rotation in the opposite
// direction. J_R corresponds
// to the input R
);
// Rotate the point x_in in a (3,) array by the rodrigues rotation in a (3,)
// array.
//
// The result is returned in a (3,) array x_out.
//
// The gradient dx_out/dr is returned in a (3,3) array J_r. Set to NULL if this
// is not wanted
//
// The gradient dx_out/dx_in is returned in a (3,3) array J_x. Set to NULL if
// this is not wanted
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_rotate_point_r( x_out,J_r,J_x,r,x_in) mrcal_rotate_point_r_full(x_out,0,J_r,0,0,J_x,0,0,r,0,x_in,0, false)
#define mrcal_rotate_point_r_inverted(x_out,J_r,J_x,r,x_in) mrcal_rotate_point_r_full(x_out,0,J_r,0,0,J_x,0,0,r,0,x_in,0, true)
void mrcal_rotate_point_r_full( // output
double* x_out, // (3,) array
int x_out_stride0, // in bytes. <= 0 means "contiguous"
double* J_r, // (3,3) array. May be NULL
int J_r_stride0, // in bytes. <= 0 means "contiguous"
int J_r_stride1, // in bytes. <= 0 means "contiguous"
double* J_x, // (3,3) array. May be NULL
int J_x_stride0, // in bytes. <= 0 means "contiguous"
int J_x_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* r, // (3,) array. May be NULL
int r_stride0, // in bytes. <= 0 means "contiguous"
const double* x_in, // (3,) array. May be NULL
int x_in_stride0, // in bytes. <= 0 means "contiguous"
bool inverted // if true, I apply a
// rotation in the opposite
// direction. J_r corresponds
// to the input r
);
// Transform the point x_in in a (3,) array by the Rt transformation in a (4,3)
// array.
//
// The result is returned in a (3,) array x_out.
//
// The gradient dx_out/dRt is returned in a (3, 4,3) array J_Rt. Set to NULL if
// this is not wanted
//
// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
// the matrix R. Set to NULL if this is not wanted
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_transform_point_Rt( x_out,J_Rt,J_x,Rt,x_in) mrcal_transform_point_Rt_full(x_out,0,J_Rt,0,0,0,J_x,0,0,Rt,0,0,x_in,0, false)
#define mrcal_transform_point_Rt_inverted(x_out,J_Rt,J_x,Rt,x_in) mrcal_transform_point_Rt_full(x_out,0,J_Rt,0,0,0,J_x,0,0,Rt,0,0,x_in,0, true)
void mrcal_transform_point_Rt_full( // output
double* x_out, // (3,) array
int x_out_stride0, // in bytes. <= 0 means "contiguous"
double* J_Rt, // (3,4,3) array. May be NULL
int J_Rt_stride0, // in bytes. <= 0 means "contiguous"
int J_Rt_stride1, // in bytes. <= 0 means "contiguous"
int J_Rt_stride2, // in bytes. <= 0 means "contiguous"
double* J_x, // (3,3) array. May be NULL
int J_x_stride0, // in bytes. <= 0 means "contiguous"
int J_x_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* Rt, // (4,3) array. May be NULL
int Rt_stride0, // in bytes. <= 0 means "contiguous"
int Rt_stride1, // in bytes. <= 0 means "contiguous"
const double* x_in, // (3,) array. May be NULL
int x_in_stride0, // in bytes. <= 0 means "contiguous"
bool inverted // if true, I apply a
// transformation in the opposite
// direction. J_Rt corresponds
// to the input Rt
);
// Transform the point x_in in a (3,) array by the rt transformation in a (6,)
// array.
//
// The result is returned in a (3,) array x_out.
//
// The gradient dx_out/drt is returned in a (3,6) array J_rt. Set to NULL if
// this is not wanted
//
// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
// the matrix R. Set to NULL if this is not wanted
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_transform_point_rt( x_out,J_rt,J_x,rt,x_in) mrcal_transform_point_rt_full(x_out,0,J_rt,0,0,J_x,0,0,rt,0,x_in,0, false)
#define mrcal_transform_point_rt_inverted(x_out,J_rt,J_x,rt,x_in) mrcal_transform_point_rt_full(x_out,0,J_rt,0,0,J_x,0,0,rt,0,x_in,0, true)
void mrcal_transform_point_rt_full( // output
double* x_out, // (3,) array
int x_out_stride0, // in bytes. <= 0 means "contiguous"
double* J_rt, // (3,6) array. May be NULL
int J_rt_stride0, // in bytes. <= 0 means "contiguous"
int J_rt_stride1, // in bytes. <= 0 means "contiguous"
double* J_x, // (3,3) array. May be NULL
int J_x_stride0, // in bytes. <= 0 means "contiguous"
int J_x_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* rt, // (6,) array. May be NULL
int rt_stride0, // in bytes. <= 0 means "contiguous"
const double* x_in, // (3,) array. May be NULL
int x_in_stride0, // in bytes. <= 0 means "contiguous"
bool inverted // if true, I apply the
// transformation in the
// opposite direction.
// J_rt corresponds to
// the input rt
);
// Convert a rotation matrix in a (3,3) array to a rodrigues vector in a (3,)
// array
//
// The result is returned in a (3,) array r
//
// The gradient dr/dR is returned in a (3, 3,3) array J. Set to NULL if this is
// not wanted
#define mrcal_r_from_R(r,J,R) mrcal_r_from_R_full(r,0,J,0,0,0,R,0,0)
void mrcal_r_from_R_full( // output
double* r, // (3,) vector
int r_stride0, // in bytes. <= 0 means "contiguous"
double* J, // (3,3,3) array. Gradient. May be NULL
int J_stride0, // in bytes. <= 0 means "contiguous"
int J_stride1, // in bytes. <= 0 means "contiguous"
int J_stride2, // in bytes. <= 0 means "contiguous"
// input
const double* R, // (3,3) array
int R_stride0, // in bytes. <= 0 means "contiguous"
int R_stride1 // in bytes. <= 0 means "contiguous"
);
// Convert a rodrigues vector in a (3,) array to a rotation matrix in a (3,3)
// array
//
// The result is returned in a (3,3) array R
//
// The gradient dR/dr is returned in a (3,3 ,3) array J. Set to NULL if this is
// not wanted
#define mrcal_R_from_r(R,J,r) mrcal_R_from_r_full(R,0,0,J,0,0,0,r,0)
void mrcal_R_from_r_full( // outputs
double* R, // (3,3) array
int R_stride0, // in bytes. <= 0 means "contiguous"
int R_stride1, // in bytes. <= 0 means "contiguous"
double* J, // (3,3,3) array. Gradient. May be NULL
int J_stride0, // in bytes. <= 0 means "contiguous"
int J_stride1, // in bytes. <= 0 means "contiguous"
int J_stride2, // in bytes. <= 0 means "contiguous"
// input
const double* r, // (3,) vector
int r_stride0 // in bytes. <= 0 means "contiguous"
);
// Invert a rotation matrix. This is a transpose
//
// The input is given in R_in in a (3,3) array
//
// The result is returned in a (3,3) array R_out
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_invert_R(R_out,R_in) mrcal_invert_R_full(R_out,0,0,R_in,0,0)
void mrcal_invert_R_full( // output
double* R_out, // (3,3) array
int R_out_stride0, // in bytes. <= 0 means "contiguous"
int R_out_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* R_in, // (3,3) array
int R_in_stride0, // in bytes. <= 0 means "contiguous"
int R_in_stride1 // in bytes. <= 0 means "contiguous"
);
// Convert an Rt transformation in a (4,3) array to an rt transformation in a
// (6,) array
//
// The result is returned in a (6,) array rt
//
// The gradient dr/dR is returned in a (3, 3,3) array J_R. Set to NULL if this
// is not wanted
//
// The t terms are identical, so dt/dt = identity and I do not return it
//
// The r and R terms are independent of the t terms, so dr/dt and dt/dR are both
// 0, and I do not return them
#define mrcal_rt_from_Rt(rt,J_R,Rt) mrcal_rt_from_Rt_full(rt,0,J_R,0,0,0,Rt,0,0)
void mrcal_rt_from_Rt_full( // output
double* rt, // (6,) vector
int rt_stride0, // in bytes. <= 0 means "contiguous"
double* J_R, // (3,3,3) array. Gradient. May be NULL
// No J_t. It's always the identity
int J_R_stride0, // in bytes. <= 0 means "contiguous"
int J_R_stride1, // in bytes. <= 0 means "contiguous"
int J_R_stride2, // in bytes. <= 0 means "contiguous"
// input
const double* Rt, // (4,3) array
int Rt_stride0, // in bytes. <= 0 means "contiguous"
int Rt_stride1 // in bytes. <= 0 means "contiguous"
);
// Convert an rt transformation in a (6,) array to an Rt transformation in a
// (4,3) array
//
// The result is returned in a (4,3) array Rt
//
// The gradient dR/dr is returned in a (3,3 ,3) array J_r. Set to NULL if this
// is not wanted
//
// The t terms are identical, so dt/dt = identity and I do not return it
//
// The r and R terms are independent of the t terms, so dR/dt and dt/dr are both
// 0, and I do not return them
#define mrcal_Rt_from_rt(Rt,J_r,rt) mrcal_Rt_from_rt_full(Rt,0,0,J_r,0,0,0,rt,0)
void mrcal_Rt_from_rt_full( // output
double* Rt, // (4,3) array
int Rt_stride0, // in bytes. <= 0 means "contiguous"
int Rt_stride1, // in bytes. <= 0 means "contiguous"
double* J_r, // (3,3,3) array. Gradient. May be NULL
// No J_t. It's just the identity
int J_r_stride0, // in bytes. <= 0 means "contiguous"
int J_r_stride1, // in bytes. <= 0 means "contiguous"
int J_r_stride2, // in bytes. <= 0 means "contiguous"
// input
const double* rt, // (6,) vector
int rt_stride0 // in bytes. <= 0 means "contiguous"
);
// Invert an Rt transformation
//
// The input is given in Rt_in in a (4,3) array
//
// The result is returned in a (4,3) array Rt_out
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_invert_Rt(Rt_out,Rt_in) mrcal_invert_Rt_full(Rt_out,0,0,Rt_in,0,0)
void mrcal_invert_Rt_full( // output
double* Rt_out, // (4,3) array
int Rt_out_stride0, // in bytes. <= 0 means "contiguous"
int Rt_out_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* Rt_in, // (4,3) array
int Rt_in_stride0, // in bytes. <= 0 means "contiguous"
int Rt_in_stride1 // in bytes. <= 0 means "contiguous"
);
// Invert an rt transformation
//
// The input is given in rt_in in a (6,) array
//
// The result is returned in a (6,) array rt_out
//
// The gradient dtout/drin is returned in a (3,3) array dtout_drin. Set to NULL
// if this is not wanted
//
// The gradient dtout/dtin is returned in a (3,3) array dtout_dtin. Set to NULL
// if this is not wanted
//
// The gradient drout/drin is always -identity. So it is not returned
//
// The gradient drout/dtin is always 0. So it is not returned
//
// In-place operation is supported; the output array may be the same as the
// input arrays to overwrite the input.
#define mrcal_invert_rt(rt_out,dtout_drin,dtout_dtin,rt_in) mrcal_invert_rt_full(rt_out,0,dtout_drin,0,0,dtout_dtin,0,0,rt_in,0)
void mrcal_invert_rt_full( // output
double* rt_out, // (6,) array
int rt_out_stride0, // in bytes. <= 0 means "contiguous"
double* dtout_drin, // (3,3) array
int dtout_drin_stride0, // in bytes. <= 0 means "contiguous"
int dtout_drin_stride1, // in bytes. <= 0 means "contiguous"
double* dtout_dtin, // (3,3) array
int dtout_dtin_stride0, // in bytes. <= 0 means "contiguous"
int dtout_dtin_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* rt_in, // (6,) array
int rt_in_stride0 // in bytes. <= 0 means "contiguous"
);
// Compose two Rt transformations
//
// Rt = Rt0 * Rt1 ---> Rt(x) = Rt0( Rt1(x) )
//
// The input transformations are given in (4,3) arrays Rt_0 and Rt_1
//
// The result is returned in a (4,3) array Rt_out
//
// In-place operation is supported; the output array may be the same as either
// of the input arrays to overwrite the input.
#define mrcal_compose_Rt(Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0)
void mrcal_compose_Rt_full( // output
double* Rt_out, // (4,3) array
int Rt_out_stride0, // in bytes. <= 0 means "contiguous"
int Rt_out_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* Rt_0, // (4,3) array
int Rt_0_stride0, // in bytes. <= 0 means "contiguous"
int Rt_0_stride1, // in bytes. <= 0 means "contiguous"
const double* Rt_1, // (4,3) array
int Rt_1_stride0, // in bytes. <= 0 means "contiguous"
int Rt_1_stride1 // in bytes. <= 0 means "contiguous"
);
// Compose two rt transformations
//
// rt = rt0 * rt1 ---> rt(x) = rt0( rt1(x) )
//
// The input transformations are given in (6,) arrays rt_0 and rt_1
//
// The result is returned in a (6,) array rt_out
//
// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this
// is not wanted
//
// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this
// is not wanted
//
// The gradient dt/dr0 is returned in a (3,3) array dt_dr0. Set to NULL if this
// is not wanted
//
// The gradient dt/dt1 is returned in a (3,3) array dt_dt1. Set to NULL if this
// is not wanted
//
// The gradients dr/dt0, dr/dt1, dt/dr1 are always 0, so they are never returned
//
// The gradient dt/dt0 is always identity, so it is never returned
//
// In-place operation is supported; the output array may be the same as either
// of the input arrays to overwrite the input.
#define mrcal_compose_rt(rt_out,dr_dr0,dr_dr1,dt_dr0,dt_dt1,rt_0,rt_1) mrcal_compose_rt_full(rt_out,0,dr_dr0,0,0,dr_dr1,0,0,dt_dr0,0,0,dt_dt1,0,0,rt_0,0,rt_1,0)
void mrcal_compose_rt_full( // output
double* rt_out, // (6,) array
int rt_out_stride0, // in bytes. <= 0 means "contiguous"
double* dr_dr0, // (3,3) array; may be NULL
int dr_dr0_stride0, // in bytes. <= 0 means "contiguous"
int dr_dr0_stride1, // in bytes. <= 0 means "contiguous"
double* dr_dr1, // (3,3) array; may be NULL
int dr_dr1_stride0, // in bytes. <= 0 means "contiguous"
int dr_dr1_stride1, // in bytes. <= 0 means "contiguous"
double* dt_dr0, // (3,3) array; may be NULL
int dt_dr0_stride0, // in bytes. <= 0 means "contiguous"
int dt_dr0_stride1, // in bytes. <= 0 means "contiguous"
double* dt_dt1, // (3,3) array; may be NULL
int dt_dt1_stride0, // in bytes. <= 0 means "contiguous"
int dt_dt1_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* rt_0, // (6,) array
int rt_0_stride0, // in bytes. <= 0 means "contiguous"
const double* rt_1, // (6,) array
int rt_1_stride0 // in bytes. <= 0 means "contiguous"
);
// Compose two angle-axis rotations
//
// r = r0 * r1 ---> r(x) = r0( r1(x) )
//
// The input rotations are given in (3,) arrays r_0 and r_1
//
// The result is returned in a (3,) array r_out
//
// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this
// is not wanted
//
// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this
// is not wanted
//
// In-place operation is supported; the output array may be the same as either
// of the input arrays to overwrite the input.
#define mrcal_compose_r(r_out,dr_dr0,dr_dr1,r_0,r_1) mrcal_compose_r_full(r_out,0,dr_dr0,0,0,dr_dr1,0,0,r_0,0,r_1,0)
void mrcal_compose_r_full( // output
double* r_out, // (3,) array
int r_out_stride0, // in bytes. <= 0 means "contiguous"
double* dr_dr0, // (3,3) array; may be NULL
int dr_dr0_stride0, // in bytes. <= 0 means "contiguous"
int dr_dr0_stride1, // in bytes. <= 0 means "contiguous"
double* dr_dr1, // (3,3) array; may be NULL
int dr_dr1_stride0, // in bytes. <= 0 means "contiguous"
int dr_dr1_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* r_0, // (3,) array
int r_0_stride0, // in bytes. <= 0 means "contiguous"
const double* r_1, // (3,) array
int r_1_stride0 // in bytes. <= 0 means "contiguous"
);
// Special-case rotation compositions for the uncertainty computation
//
// Same as mrcal_compose_r() except
//
// - r0 is assumed to be 0, so we don't ingest it, and we don't report the
// composition result
// - we ONLY report the dr01/dr0 gradient
//
// In python this function is equivalent to
//
// _,dr01_dr0,_ = compose_r(np.zeros((3,),),
// r1,
// get_gradients=True)
#define mrcal_compose_r_tinyr0_gradientr0(dr_dr0,r_1) \
mrcal_compose_r_tinyr0_gradientr0_full(dr_dr0,0,0,r_1,0)
void mrcal_compose_r_tinyr0_gradientr0_full( // output
double* dr_dr0, // (3,3) array; may be NULL
int dr_dr0_stride0, // in bytes. <= 0 means "contiguous"
int dr_dr0_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* r_1, // (3,) array
int r_1_stride0 // in bytes. <= 0 means "contiguous"
);
// Same as mrcal_compose_r() except
//
// - r1 is assumed to be 0, so we don't ingest it, and we don't report the
// composition result
// - we ONLY report the dr01/dr1 gradient
//
// In python this function is equivalent to
//
// _,_,dr01_dr1 = compose_r(r0,
// np.zeros((3,),),
// get_gradients=True)
#define mrcal_compose_r_tinyr1_gradientr1(dr_dr1,r_0) \
mrcal_compose_r_tinyr1_gradientr1_full(dr_dr1,0,0,r_0,0)
void mrcal_compose_r_tinyr1_gradientr1_full( // output
double* dr_dr1, // (3,3) array; may be NULL
int dr_dr1_stride0, // in bytes. <= 0 means "contiguous"
int dr_dr1_stride1, // in bytes. <= 0 means "contiguous"
// input
const double* r_0, // (3,) array
int r_0_stride0 // in bytes. <= 0 means "contiguous"
);
// Procrustes fit functions. Align two corresponding sets of normalized
// direction vectors or points. Return true on success
bool mrcal_align_procrustes_vectors_R01(// out
double* R01,
// in
const int N,
// (N,3) arrays
// normalized direction vectors
const double* v0,
const double* v1,
// (N,) array; may be NULL to use an even
// weighting
const double* weights);
bool mrcal_align_procrustes_points_Rt01(// out
double* Rt01,
// in
const int N,
// (N,3) arrays
const double* p0,
const double* p1,
// (N,) array; may be NULL to use an even
// weighting
const double* weights);
// Compute a non-unique rotation to map a given vector to [0,0,1]
void mrcal_R_aligned_to_vector(// out
double* R,
// in
const double* v);

43
wpical/src/main/native/thirdparty/mrcal/include/scales.h vendored Normal file
View File

@@ -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

129
wpical/src/main/native/thirdparty/mrcal/include/stereo.h vendored Normal file
View File

@@ -0,0 +1,129 @@
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
#pragma once
#include "mrcal-types.h"
#include "mrcal-image.h"
// The reference implementation in Python is _rectified_resolution_python() in
// stereo.py
//
// The Python wrapper is mrcal.rectified_resolution(), and the documentation is
// in the docstring of that function
bool mrcal_rectified_resolution( // output and input
// > 0: use given value
// < 0: autodetect and scale
double* pixels_per_deg_az,
double* pixels_per_deg_el,
// input
const mrcal_lensmodel_t* lensmodel,
const double* intrinsics,
const mrcal_point2_t* azel_fov_deg,
const mrcal_point2_t* azel0_deg,
const double* R_cam0_rect0,
const mrcal_lensmodel_type_t rectification_model_type);
// The reference implementation in Python is _rectified_system_python() in
// stereo.py
//
// The Python wrapper is mrcal.rectified_system(), and the documentation is in
// the docstring of that function
bool mrcal_rectified_system(// output
unsigned int* imagersize_rectified,
double* fxycxy_rectified,
double* rt_rect0_ref,
double* baseline,
// input, output
// > 0: use given value
// < 0: autodetect and scale
double* pixels_per_deg_az,
double* pixels_per_deg_el,
// input, output
// if(..._autodetect) { the results are returned here }
mrcal_point2_t* azel_fov_deg,
mrcal_point2_t* azel0_deg,
// input
const mrcal_lensmodel_t* lensmodel0,
const double* intrinsics0,
const double* rt_cam0_ref,
const double* rt_cam1_ref,
const mrcal_lensmodel_type_t rectification_model_type,
bool az0_deg_autodetect,
bool el0_deg_autodetect,
bool az_fov_deg_autodetect,
bool el_fov_deg_autodetect);
// The reference implementation in Python is _rectification_maps_python() in
// stereo.py
//
// The Python wrapper is mrcal.rectification_maps(), and the documentation is in
// the docstring of that function
bool mrcal_rectification_maps(// output
// Dense array of shape (Ncameras=2, Nel, Naz, Nxy=2)
float* rectification_maps,
// input
const mrcal_lensmodel_t* lensmodel0,
const double* intrinsics0,
const double* r_cam0_ref,
const mrcal_lensmodel_t* lensmodel1,
const double* intrinsics1,
const double* r_cam1_ref,
const mrcal_lensmodel_type_t rectification_model_type,
const double* fxycxy_rectified,
const unsigned int* imagersize_rectified,
const double* r_rect0_ref);
// The reference implementation in Python is _stereo_range_python() in
// stereo.py
//
// The Python wrapper is mrcal.stereo_range(), and the documentation is in
// the docstring of that function
bool mrcal_stereo_range_sparse(// output
double* range, // N of these
// input
const double* disparity, // N of these
const mrcal_point2_t* qrect0, // N of these
const int N, // how many points
const double disparity_min,
const double disparity_max,
// models_rectified
const mrcal_lensmodel_type_t rectification_model_type,
const double* fxycxy_rectified,
const double baseline);
bool mrcal_stereo_range_dense(// output
mrcal_image_double_t* range,
// input
const mrcal_image_uint16_t* disparity_scaled,
const uint16_t disparity_scale,
// Used to detect invalid values. Set to
// 0,UINT16_MAX to ignore
const uint16_t disparity_scaled_min,
const uint16_t disparity_scaled_max,
// models_rectified
const mrcal_lensmodel_type_t rectification_model_type,
const double* fxycxy_rectified,
const double baseline);

42
wpical/src/main/native/thirdparty/mrcal/include/strides.h vendored Normal file
View File

@@ -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

153
wpical/src/main/native/thirdparty/mrcal/include/triangulation.h vendored Normal file
View File

@@ -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);

13
wpical/src/main/native/thirdparty/mrcal/include/util.h vendored Normal file
View File

@@ -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 <stdio.h>
#define MSG(fmt, ...) fprintf(stderr, "%s(%d): " fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__)

332
wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp vendored Normal file
View File

@@ -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 <stdio.h>
#include <assert.h>
#include "autodiff.hh"
extern "C" {
#include "cahvore.h"
}
template <int N>
static
bool _project_cahvore_internals( // outputs
vec_withgrad_t<N,3>* pdistorted,
// inputs
const vec_withgrad_t<N,3>& p,
const val_withgrad_t<N>& alpha,
const val_withgrad_t<N>& beta,
const val_withgrad_t<N>& r0,
const val_withgrad_t<N>& r1,
const val_withgrad_t<N>& r2,
const val_withgrad_t<N>& e0,
const val_withgrad_t<N>& e1,
const val_withgrad_t<N>& 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<N,2> sca = alpha.sincos();
vec_withgrad_t<N,2> scb = beta .sincos();
vec_withgrad_t<N,3> 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<N> zeta = p.dot(o);
// Basic Computations
// Calculate initial terms
vec_withgrad_t<N,3> u = o*zeta;
vec_withgrad_t<N,3> ll = p - u;
val_withgrad_t<N> l = ll.mag();
// Calculate theta using Newton's Method
val_withgrad_t<N> theta = l.atan2(zeta);
int inewton;
for( inewton = 100; inewton; inewton--)
{
// Compute terms from the current value of theta
vec_withgrad_t<N,2> scth = theta.sincos();
val_withgrad_t<N> theta2 = theta * theta;
val_withgrad_t<N> theta3 = theta * theta2;
val_withgrad_t<N> theta4 = theta * theta3;
val_withgrad_t<N> 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<N> 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<N> linth = theta * cahvore_linearity;
val_withgrad_t<N> 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<N> chi2 = chi * chi;
val_withgrad_t<N> chi3 = chi * chi2;
val_withgrad_t<N> chi4 = chi * chi3;
val_withgrad_t<N> zetap = l / chi;
val_withgrad_t<N> mu = r0 + r1*chi2 + r2*chi4;
vec_withgrad_t<N,3> uu = o*zetap;
vec_withgrad_t<N,3> vv = ll * (mu + 1.);
*pdistorted = uu + vv;
}
else
*pdistorted = p;
return true;
}
// Not meant to be touched by the end user. Implemented separate from mrcal.c so
// that I can get automated gradient propagation with c++
extern "C"
bool project_cahvore_internals( // outputs
mrcal_point3_t* __restrict pdistorted,
double* __restrict dpdistorted_dintrinsics_nocore,
double* __restrict dpdistorted_dp,
// inputs
const mrcal_point3_t* __restrict p,
const double* __restrict intrinsics_nocore,
const double cahvore_linearity)
{
if( dpdistorted_dintrinsics_nocore == NULL &&
dpdistorted_dp == NULL )
{
// No gradients. NGRAD in all the templates is 0
vec_withgrad_t<0,3> pdistortedg;
vec_withgrad_t<0,8> ig;
ig.init_vars(intrinsics_nocore,
0,8, // ivar0,Nvars
-1 // i_gradvec0
);
vec_withgrad_t<0,3> pg;
pg.init_vars(p->xyz,
0,3, // ivar0,Nvars
-1 // i_gradvec0
);
if(!_project_cahvore_internals(&pdistortedg,
pg,
ig[0],
ig[1],
ig[2],
ig[3],
ig[4],
ig[5],
ig[6],
ig[7],
cahvore_linearity))
return false;
pdistortedg.extract_value(pdistorted->xyz);
return true;
}
if( dpdistorted_dintrinsics_nocore == NULL &&
dpdistorted_dp != NULL )
{
// 3 variables: p (3 vars)
vec_withgrad_t<3,3> pdistortedg;
vec_withgrad_t<3,8> ig;
ig.init_vars(intrinsics_nocore,
0,8, // ivar0,Nvars
-1 // i_gradvec0
);
vec_withgrad_t<3,3> pg;
pg.init_vars(p->xyz,
0,3, // ivar0,Nvars
0 // i_gradvec0
);
if(!_project_cahvore_internals(&pdistortedg,
pg,
ig[0],
ig[1],
ig[2],
ig[3],
ig[4],
ig[5],
ig[6],
ig[7],
cahvore_linearity))
return false;
pdistortedg.extract_value(pdistorted->xyz);
pdistortedg.extract_grad (dpdistorted_dp,
0,3, // ivar0,Nvars
0, // i_gradvec0
sizeof(double)*3, sizeof(double),
3 // Nvars
);
return true;
}
if( dpdistorted_dintrinsics_nocore != NULL &&
dpdistorted_dp == NULL )
{
// 8 variables: alpha,beta,R,E (8 vars)
vec_withgrad_t<8,3> pdistortedg;
vec_withgrad_t<8,8> ig;
ig.init_vars(intrinsics_nocore,
0,8, // ivar0,Nvars
0 // i_gradvec0
);
vec_withgrad_t<8,3> pg;
pg.init_vars(p->xyz,
0,3, // ivar0,Nvars
-1 // i_gradvec0
);
if(!_project_cahvore_internals(&pdistortedg,
pg,
ig[0],
ig[1],
ig[2],
ig[3],
ig[4],
ig[5],
ig[6],
ig[7],
cahvore_linearity))
return false;
pdistortedg.extract_value(pdistorted->xyz);
pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore,
0,8, // i_gradvec0,N_gradout
0, // ivar0
sizeof(double)*8, sizeof(double),
3 // Nvars
);
return true;
}
if( dpdistorted_dintrinsics_nocore != NULL &&
dpdistorted_dp != NULL )
{
// 11 variables: alpha,beta,R,E (8 vars) + p (3 vars)
vec_withgrad_t<11,3> pdistortedg;
vec_withgrad_t<11,8> ig;
ig.init_vars(intrinsics_nocore,
0,8, // ivar0,Nvars
0 // i_gradvec0
);
vec_withgrad_t<11,3> pg;
pg.init_vars(p->xyz,
0,3, // ivar0,Nvars
8 // i_gradvec0
);
if(!_project_cahvore_internals(&pdistortedg,
pg,
ig[0],
ig[1],
ig[2],
ig[3],
ig[4],
ig[5],
ig[6],
ig[7],
cahvore_linearity))
return false;
pdistortedg.extract_value(pdistorted->xyz);
pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore,
0,8, // i_gradvec0,N_gradout
0, // ivar0
sizeof(double)*8, sizeof(double),
3 // Nvars
);
pdistortedg.extract_grad (dpdistorted_dp,
8,3, // ivar0,Nvars
0, // i_gradvec0
sizeof(double)*3, sizeof(double),
3 // Nvars
);
return true;
}
fprintf(stderr, "Getting here is a bug. Please report\n");
assert(0);
}

463
wpical/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl vendored Executable file
View File

@@ -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 <<EOC;
static inline double dot_vec(int n, const double* restrict a, const double* restrict b)
{
double dot = 0.0;
for(int i=0; i<n; i++)
dot += a[i]*b[i];
return dot;
}
EOC
}
sub norms
{
print <<EOC;
static inline double norm2_vec(int n, const double* restrict a)
{
double dot = 0.0;
for(int i=0; i<n; i++)
dot += a[i]*a[i];
return dot;
}
EOC
}
sub vectorArithmetic
{
my $vout = <<EOC;
// a + b -> 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<n; i++)
vout[i] = a[i] + b[i];
}
// a - b -> 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<n; i++)
vout[i] = a[i] - b[i];
}
EOC
print $vout;
print _makeScaled_arithmetic($vout);
my $arg0 = _getFirstDataArg($vout);
my $vinplace = _makeInplace_mulVector($vout, $arg0);
print $vinplace;
print _makeScaled_arithmetic($vinplace);
print _makeVaccum($vout);
}
sub matrixVectorSym
{
my $n = shift;
my $vout = <<EOC;
// $n-vector by symmetric ${n}x$n
static inline void mul_vec${n}_sym$n${n}_vout(const double* restrict v, const double* restrict s, double* restrict vout)
{
EOC
# I now have the header, opening brace. Writing each row element output
my %isymHash = (next => 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 = <<EOC;
// $n-vector by ${n}x$m matrix multiplication
static inline void mul_vec${n}_gen$n${m}_vout(const double* restrict v, const double* restrict m, double* restrict vout)
{
EOC
# I now have the header, opening brace. Writing each row element output
for my $i(0..$m-1)
{
my @js = 0..$n-1;
my @im = map {$i + $_*$m} @js;
our ($a,$b);
my @sum_components = pairwise {"m[$a]*v[$b]"} @im, @js;
$vout .= " vout[$i] = " . join(' + ', @sum_components) . ";\n";
}
$vout .= "}";
print _multiplicationVersions($vout, $m, $n);
# now the transposed version
$vout = <<EOC;
// $n-vector by ${m}x$n-transposed matrix multiplication
static inline void mul_vec${n}_gen$m${n}t_vout(const double* restrict v, const double* restrict mt, double* restrict vout)
{
EOC
# I now have the header, opening brace. Writing each row element output
for my $i(0..$m-1)
{
my @js = 0..$n-1;
my @im = map {$i*$n + $_} @js;
our ($a,$b);
my @sum_components = pairwise {"mt[$a]*v[$b]"} @im, @js;
$vout .= " vout[$i] = " . join(' + ', @sum_components) . ";\n";
}
$vout .= "}";
print _multiplicationVersions($vout, $m,$n);
}
sub matrixMatrixSym
{
my $n = 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 = <<EOC;
// general Nx$n matrix by symmetric ${n}x$n
static inline void mul_genN${n}_sym${n}${n}_vout(int n, const double* restrict v, const double* restrict s, double* restrict vout)
{
for(int i=0; i<n; i++)
mul_vec${n}_sym${n}${n}_vout(v + $n*i, s, vout + $n*i);
}
EOC
print _multiplicationVersions($vout);
}
sub matrixMatrixMatrixSym
{
my $n = shift;
die 'matrixMatrixMatrixSym ONLY defined for $n==3 right now' if $n != 3;
print <<'EOC';
// (%i2) sym3 : matrix([m0,m1,m2],
// [m1,m3,m4],
// [m2,m4,m5]);
// (%o2) matrix([m0,m1,m2],[m1,m3,m4],[m2,m4,m5])
// (%i3) sym3_a : matrix([a0,a1,a2],
// [a1,a3,a4],
// [a2,a4,a5]);
// (%o3) matrix([a0,a1,a2],[a1,a3,a4],[a2,a4,a5])
// (%i4) sym3_b : matrix([b0,b1,b2],
// [b1,b3,b4],
// [b2,b4,b5]);
// (%o4) matrix([b0,b1,b2],[b1,b3,b4],[b2,b4,b5])
// (%i5) sym3_a . sym3_b . sym3_a;
// (%o5) matrix([a2*(a2*b5+a1*b4+a0*b2)+a1*(a2*b4+a1*b3+a0*b1) + a0*(a2*b2+a1*b1+a0*b0), a2*(a4*b5+a3*b4+a1*b2)+a1*(a4*b4+a3*b3+a1*b1) + a0*(a4*b2+a3*b1+a1*b0), a2*(a5*b5+a4*b4+a2*b2)+a1*(a5*b4+a4*b3+a2*b1) + a0*(a5*b2+a4*b1+a2*b0)],
// [a4*(a2*b5+a1*b4+a0*b2)+a3*(a2*b4+a1*b3+a0*b1) + a1*(a2*b2+a1*b1+a0*b0), a4*(a4*b5+a3*b4+a1*b2)+a3*(a4*b4+a3*b3+a1*b1) + a1*(a4*b2+a3*b1+a1*b0), a4*(a5*b5+a4*b4+a2*b2)+a3*(a5*b4+a4*b3+a2*b1) + a1*(a5*b2+a4*b1+a2*b0)],
// [a5*(a2*b5+a1*b4+a0*b2)+a4*(a2*b4+a1*b3+a0*b1) + a2*(a2*b2+a1*b1+a0*b0), a5*(a4*b5+a3*b4+a1*b2)+a4*(a4*b4+a3*b3+a1*b1) + a2*(a4*b2+a3*b1+a1*b0), a5*(a5*b5+a4*b4+a2*b2)+a4*(a5*b4+a4*b3+a2*b1) + a2*(a5*b2+a4*b1+a2*b0)])
EOC
my $vout = <<'EOC';
// symmetric A * B * A
static inline void mul_sym33_sym33_sym33_vout(const double* restrict a, const double* restrict b, double* restrict vout)
{
double t0 = a[2]*b[5]+a[1]*b[4]+a[0]*b[2];
double t1 = a[2]*b[4]+a[1]*b[3]+a[0]*b[1];
double t2 = a[2]*b[2]+a[1]*b[1]+a[0]*b[0];
double t3 = a[4]*b[2]+a[3]*b[1]+a[1]*b[0];
double t4 = a[4]*b[5]+a[3]*b[4]+a[1]*b[2];
double t5 = a[4]*b[4]+a[3]*b[3]+a[1]*b[1];
vout[0] = a[2]*t0+a[1]*t1+a[0]*t2;
vout[1] = a[4]*t0+a[3]*t1+a[1]*t2;
vout[2] = a[5]*t0+a[4]*t1+a[2]*t2;
vout[3] = a[4]*t4+a[3]*t5+a[1]*t3;
vout[4] = a[5]*t4+a[4]*t5+a[2]*t3;
vout[5] = a[5]*(a[5]*b[5]+a[4]*b[4]+a[2]*b[2])+a[4]*(a[5]*b[4]+a[4]*b[3]+a[2]*b[1]) + a[2]*(a[5]*b[2]+a[4]*b[1]+a[2]*b[0]);
}
EOC
print $vout;
}
sub matrixMatrixGen
{
my $n = 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 = <<EOC;
// general Nx${n} matrix by general ${n}x${n}
static inline void mul_genN${n}_gen${n}${n}_vout(int n, const double* restrict v, const double* restrict m, double* restrict vout)
{
for(int i=0; i<n; i++)
mul_vec${n}_gen${n}${n}_vout(v + $n*i, m, vout + $n*i);
}
// general Nx${n} matrix by general ${n}x${n}
static inline void mul_genN${n}_gen${n}${n}t_vout(int n, const double* restrict v, const double* restrict mt, double* restrict vout)
{
for(int i=0; i<n; i++)
mul_vec${n}_gen${n}${n}t_vout(v + $n*i, mt, vout + $n*i);
}
EOC
print _multiplicationVersions($vout);
}
sub _multiplicationVersions
{
my $vout = shift;
my $n = shift;
my $m = shift;
my $arg0 = _getFirstDataArg($vout);
my $funcs = $vout . "\n";
$funcs .= (defined $n ?
_makeInplace_mulVector($vout, $arg0, $n, $m) : _makeInplace_mulMatrix($vout) ) . "\n";
$funcs .= _makeVaccum ($vout) . "\n";
$funcs .= (defined $n ?
_makeScaled_mulVector ($funcs) : _makeScaled_mulMatrix ($funcs) ) . "\n";
return $funcs;
}
sub _getSymmetricIndices_row
{
my $hash = shift;
my $i = shift;
my $n = shift;
my @isym;
for my $j (0..$n-1)
{
my $key = join(',', sort($i,$j));
if( !exists $hash->{$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;
}

6890
wpical/src/main/native/thirdparty/mrcal/src/mrcal.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

152
wpical/src/main/native/thirdparty/mrcal/src/opencv.c vendored Normal file
View File

@@ -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<Nintrinsics-4; i++)
k[i] = intrinsics[i+4];
for( int i = 0; i < N; i++ )
{
double z_recip = 1./p[i].z;
double x = p[i].x * z_recip;
double y = p[i].y * z_recip;
double r2 = x*x + y*y;
double r4 = r2*r2;
double r6 = r4*r2;
double a1 = 2*x*y;
double a2 = r2 + 2*x*x;
double a3 = r2 + 2*y*y;
double cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
double icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
double xd = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
double yd = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
q[i].x = xd*fx + cx;
q[i].y = yd*fy + cy;
if( dq_dp )
{
double dx_dp[] = { z_recip, 0, -x*z_recip };
double dy_dp[] = { 0, z_recip, -y*z_recip };
for( int j = 0; j < 3; j++ )
{
double dr2_dp = 2*x*dx_dp[j] + 2*y*dy_dp[j];
double dcdist_dp = k[0]*dr2_dp + 2*k[1]*r2*dr2_dp + 3*k[4]*r4*dr2_dp;
double dicdist2_dp = -icdist2*icdist2*(k[5]*dr2_dp + 2*k[6]*r2*dr2_dp + 3*k[7]*r4*dr2_dp);
double da1_dp = 2*(x*dy_dp[j] + y*dx_dp[j]);
double dmx_dp = (dx_dp[j]*cdist*icdist2 + x*dcdist_dp*icdist2 + x*cdist*dicdist2_dp +
k[2]*da1_dp + k[3]*(dr2_dp + 4*x*dx_dp[j]) + k[8]*dr2_dp + 2*r2*k[9]*dr2_dp);
double dmy_dp = (dy_dp[j]*cdist*icdist2 + y*dcdist_dp*icdist2 + y*cdist*dicdist2_dp +
k[2]*(dr2_dp + 4*y*dy_dp[j]) + k[3]*da1_dp + k[10]*dr2_dp + 2*r2*k[11]*dr2_dp);
dq_dp[i*2 + 0].xyz[j] = fx*dmx_dp;
dq_dp[i*2 + 1].xyz[j] = fy*dmy_dp;
}
}
if( dq_dintrinsics_nocore )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 0] = fx*x*icdist2*r2;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 0] = fy*(y*icdist2*r2);
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 1] = fx*x*icdist2*r4;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 1] = fy*y*icdist2*r4;
if( Nintrinsics-4 > 2 )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 2] = fx*a1;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 2] = fy*a3;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 3] = fx*a2;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 3] = fy*a1;
if( Nintrinsics-4 > 4 )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 4] = fx*x*icdist2*r6;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 4] = fy*y*icdist2*r6;
if( Nintrinsics-4 > 5 )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 5] = fx*x*cdist*(-icdist2)*icdist2*r2;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 5] = fy*y*cdist*(-icdist2)*icdist2*r2;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 6] = fx*x*cdist*(-icdist2)*icdist2*r4;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 6] = fy*y*cdist*(-icdist2)*icdist2*r4;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 7] = fx*x*cdist*(-icdist2)*icdist2*r6;
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 7] = fy*y*cdist*(-icdist2)*icdist2*r6;
if( Nintrinsics-4 > 8 )
{
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 8] = fx*r2; //s1
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 8] = fy*0; //s1
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 9] = fx*r4; //s2
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 9] = fy*0; //s2
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 10] = fx*0; //s3
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 10] = fy*r2; //s3
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 11] = fx*0; //s4
dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 11] = fy*r4; //s4
}
}
}
}
}
}
}

155
wpical/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c vendored Normal file
View File

@@ -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 <math.h>
#include <float.h>
#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]);
}
}

891
wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp vendored Normal file
View File

@@ -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<int N>
static void
rotate_point_r_core(// output
val_withgrad_t<N>* x_outg,
// inputs
const val_withgrad_t<N>* rg,
const val_withgrad_t<N>* 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<N> th2 =
rg[0]*rg[0] +
rg[1]*rg[1] +
rg[2]*rg[2];
const val_withgrad_t<N> 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<N> 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<N> th = th2.sqrt();
const vec_withgrad_t<N, 2> 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<N>(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<int N>
static void
compose_r_core(// output
vec_withgrad_t<N, 3>* r,
// inputs
const vec_withgrad_t<N, 3>* r0,
const vec_withgrad_t<N, 3>* 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<N> A = r0->mag() / 2.;
const val_withgrad_t<N> B = r1->mag() / 2.;
const val_withgrad_t<N> inner = r0->dot(*r1);
const vec_withgrad_t<N, 3> 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<N>& B_over_tanB = B / B.tan();
for(int i=0; i<3; i++)
(*r)[i] =
(*r1)[i] * (val_withgrad_t<N>(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<N>& A_over_tanA = A / A.tan();
for(int i=0; i<3; i++)
(*r)[i] =
(*r0)[i] * (val_withgrad_t<N>(1.0)
- inner * (A_over_tanA - 1.) / (A*A*4.))
+ (*r1)[i] * A_over_tanA
+ cross[i] / 2.;
return;
}
const vec_withgrad_t<N, 2> sincosA = A.sincos();
const vec_withgrad_t<N, 2> sincosB = B.sincos();
const val_withgrad_t<N>& sinA = sincosA.v[0];
const val_withgrad_t<N>& cosA = sincosA.v[1];
const val_withgrad_t<N>& sinB = sincosB.v[0];
const val_withgrad_t<N>& cosB = sincosB.v[1];
const val_withgrad_t<N>& sinA_over_A = A.sinx_over_x(sinA);
const val_withgrad_t<N>& sinB_over_B = B.sinx_over_x(sinB);
val_withgrad_t<N> 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<N> C = cosC.acos();
const val_withgrad_t<N> sinC = (val_withgrad_t<N>(1.) - cosC*cosC).sqrt();
for(int i=0; i<3; i++)
(*r)[i] *= (C - M_PI) / sinC;
}
else
{
// nominal case
const val_withgrad_t<N> C = cosC.acos();
const val_withgrad_t<N> sinC = (val_withgrad_t<N>(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);
}
}

1129
wpical/src/main/native/thirdparty/mrcal/src/poseutils.c vendored Normal file
View File

File diff suppressed because it is too large Load Diff

1032
wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp vendored Normal file
View File

File diff suppressed because it is too large Load Diff

84
wpical/src/main/native/thirdparty/mrcal_java/include/mrcal_wrapper.h vendored Normal file
View File

@@ -0,0 +1,84 @@
/*
* Copyright (C) Photon Vision.
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
* SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
* OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
#pragma once
extern "C" {
// Seems to be missing C++ guards
#include <mrcal.h>
} // extern "C"
// Seems like these people don't properly extern-c their headers either
extern "C" {
#include <suitesparse/SuiteSparse_config.h>
#include <suitesparse/cholmod_core.h>
} // extern "C"
#include <memory>
#include <opencv2/opencv.hpp>
#include <span>
#include <utility>
#include <vector>
struct mrcal_result {
bool success;
std::vector<double> intrinsics;
double rms_error;
std::vector<double> residuals;
mrcal_calobject_warp_t calobject_warp;
int Noutliers_board;
// TODO standard devs
mrcal_result() = default;
mrcal_result(bool success_, std::vector<double> intrinsics_,
double rms_error_, std::vector<double> residuals_,
mrcal_calobject_warp_t calobject_warp_, int Noutliers_board_)
: success{success_}, intrinsics{std::move(intrinsics_)},
rms_error{rms_error_}, residuals{std::move(residuals_)},
calobject_warp{calobject_warp_}, Noutliers_board{Noutliers_board_} {}
mrcal_result(mrcal_result &&) = delete;
~mrcal_result();
};
mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool,
cv::Size boardSize, cv::Size imagerSize,
double squareSize, double focal_len_guess);
std::unique_ptr<mrcal_result> mrcal_main(
// List, depth is ordered array observation[N frames, object_height,
// object_width] = [x,y, weight] weight<0 means ignored)
std::span<mrcal_point3_t> observations_board,
// RT transform from camera to object
std::span<mrcal_pose_t> frames_rt_toref,
// Chessboard size, in corners (not squares)
cv::Size calobjectSize, double boardSpacing,
// res, pixels
cv::Size cameraRes,
// focal length, in pixels
double focal_len_guess);
enum class CameraLensModel {
LENSMODEL_OPENCV5 = 0,
LENSMODEL_OPENCV8,
LENSMODEL_STEREOGRAPHIC,
LENSMODEL_SPLINED_STEREOGRAPHIC
};
bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat,
const cv::Mat *distCoeffs, CameraLensModel lensModel,
// Extra stuff for splined stereographic models
uint16_t order, uint16_t Nx, uint16_t Ny,
uint16_t fov_x_deg);

614
wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp vendored Normal file
View File

@@ -0,0 +1,614 @@
/*
* Copyright (C) Photon Vision.
*
* Permission to use, copy, modify, and/or distribute this software for any
* purpose with or without fee is hereby granted.
*
* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
* SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
* OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
* CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
*/
#include "mrcal_wrapper.h"
#include <stdint.h>
#include <algorithm>
#include <chrono>
#include <cstdio>
#include <random>
#include <span>
#include <stdexcept>
#include <vector>
using namespace cv;
class CholmodCtx {
public:
cholmod_common Common, *cc;
CholmodCtx() {
cc = &Common;
cholmod_l_start(cc);
}
~CholmodCtx() { cholmod_l_finish(cc); }
};
static CholmodCtx cctx;
#define BARF(...) std::fprintf(stderr, __VA_ARGS__)
// forward declarations for functions borrowed from mrcal-pywrap
static mrcal_problem_selections_t construct_problem_selections(
int do_optimize_intrinsics_core, int do_optimize_intrinsics_distortions,
int do_optimize_extrinsics, int do_optimize_frames,
int do_optimize_calobject_warp, int do_apply_regularization,
int do_apply_outlier_rejection, int Ncameras_intrinsics,
int Ncameras_extrinsics, int Nframes, int Nobservations_board);
bool lensmodel_one_validate_args(mrcal_lensmodel_t *mrcal_lensmodel,
std::vector<double> intrinsics,
bool do_check_layout);
mrcal_point3_t* observations_point = nullptr;
mrcal_pose_t*
extrinsics_rt_fromref = nullptr; // Always zero for single camera, it seems?
mrcal_point3_t* points = nullptr; // Seems to always to be None for single camera?
static std::unique_ptr<mrcal_result> mrcal_calibrate(
// List, depth is ordered array observation[N frames, object_height,
// object_width] = [x,y, weight] weight<0 means ignored)
std::span<mrcal_point3_t> observations_board,
// RT transform from camera to object
std::span<mrcal_pose_t> frames_rt_toref,
// Chessboard size, in corners (not squares)
Size calobjectSize, double calibration_object_spacing,
// res, pixels
Size cameraRes,
// solver options
mrcal_problem_selections_t problem_selections,
// seed intrinsics/cameramodel to optimize for
mrcal_lensmodel_t mrcal_lensmodel, std::vector<double> intrinsics) {
// Number of board observations we've got. List of boards. in python, it's
// (number of chessboard pictures) x (rows) x (cos) x (3)
// hard-coded to 8, since that's what I've got below
int Nobservations_board = frames_rt_toref.size();
// Looks like this is hard-coded to 0 in Python for initial single-camera
// solve?
int Nobservations_point = 0;
int calibration_object_width_n =
calobjectSize.width; // Object width, in # of corners
int calibration_object_height_n =
calobjectSize.height; // Object height, in # of corners
// TODO set sizes and populate
int imagersize[] = {cameraRes.width, cameraRes.height};
mrcal_calobject_warp_t calobject_warp = {0, 0};
// int Nobservations_point_triangulated = 0; // no clue what this is
int Npoints = 0; // seems like this is also unused? whack
int Npoints_fixed = 0; // seems like this is also unused? whack
// Number of cameras to solve for intrinsics
int Ncameras_intrinsics = 1;
// Hard-coded to match out 8 frames from above (borrowed from python)
std::vector<mrcal_point3_t> indices_frame_camintrinsics_camextrinsics;
// Frame index, camera number, (camera number)-1???
for (int i = 0; i < Nobservations_board; i++) {
indices_frame_camintrinsics_camextrinsics.push_back(
{static_cast<double>(i), 0, -1});
}
// Pool is the raw observation backing array
mrcal_point3_t *c_observations_board_pool = (observations_board.data());
mrcal_point3_t *c_observations_point_pool = observations_point;
// Copy from board/point pool above, using some code borrowed from
// mrcal-pywrap
std::vector<mrcal_observation_board_t> observations_board_data(Nobservations_board);
auto c_observations_board = observations_board_data.data();
// Try to make sure we don't accidentally make a zero-length array or
// something stupid
std::vector<mrcal_observation_point_t>
observations_point_data(std::max(Nobservations_point, 1));
mrcal_observation_point_t*
c_observations_point = observations_point_data.data();
for (int i_observation = 0; i_observation < Nobservations_board;
i_observation++) {
int32_t iframe =
indices_frame_camintrinsics_camextrinsics.at(i_observation).x;
int32_t icam_intrinsics =
indices_frame_camintrinsics_camextrinsics.at(i_observation).y;
int32_t icam_extrinsics =
indices_frame_camintrinsics_camextrinsics.at(i_observation).z;
c_observations_board[i_observation].icam.intrinsics = icam_intrinsics;
c_observations_board[i_observation].icam.extrinsics = icam_extrinsics;
c_observations_board[i_observation].iframe = iframe;
}
for (int i_observation = 0; i_observation < Nobservations_point;
i_observation++) {
int32_t i_point =
indices_frame_camintrinsics_camextrinsics.at(i_observation).x;
int32_t icam_intrinsics =
indices_frame_camintrinsics_camextrinsics.at(i_observation).y;
int32_t icam_extrinsics =
indices_frame_camintrinsics_camextrinsics.at(i_observation).z;
c_observations_point[i_observation].icam.intrinsics = icam_intrinsics;
c_observations_point[i_observation].icam.extrinsics = icam_extrinsics;
c_observations_point[i_observation].i_point = i_point;
}
int Ncameras_extrinsics = 0; // Seems to always be zero for single camera
int Nframes =
frames_rt_toref.size(); // Number of pictures of the object we've got
mrcal_observation_point_triangulated_t *observations_point_triangulated = NULL;
// NULL;
if (!lensmodel_one_validate_args(&mrcal_lensmodel, intrinsics, false)) {
auto ret = std::make_unique<mrcal_result>();
ret->success = false;
return ret;
}
int Nstate = mrcal_num_states(
Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed,
Nobservations_board, problem_selections, &mrcal_lensmodel);
int Nmeasurements = mrcal_num_measurements(
Nobservations_board, Nobservations_point,
observations_point_triangulated,
0, // hard-coded to 0
calibration_object_width_n, calibration_object_height_n,
Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed,
problem_selections, &mrcal_lensmodel);
// OK, now we should have everything ready! Just some final setup and then
// call optimize
// Residuals
std::vector<double> b_packed_final(Nstate);
auto c_b_packed_final = b_packed_final.data();
std::vector<double> x_final(Nmeasurements);
auto c_x_final = x_final.data();
// Seeds
double *c_intrinsics = intrinsics.data();
mrcal_pose_t *c_extrinsics = extrinsics_rt_fromref;
mrcal_pose_t *c_frames = frames_rt_toref.data();
mrcal_point3_t *c_points = points;
mrcal_calobject_warp_t *c_calobject_warp = &calobject_warp;
// in
int *c_imagersizes = imagersize;
auto point_min_range = -1.0, point_max_range = -1.0;
mrcal_problem_constants_t problem_constants = {
.point_min_range = point_min_range, .point_max_range = point_max_range};
int verbose = 0;
auto stats = mrcal_optimize(
NULL, -1, c_x_final, Nmeasurements * sizeof(double), c_intrinsics,
c_extrinsics, c_frames, c_points, c_calobject_warp, Ncameras_intrinsics,
Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed,
c_observations_board, c_observations_point, Nobservations_board,
Nobservations_point,
observations_point_triangulated, -1,
c_observations_board_pool, c_observations_point_pool, &mrcal_lensmodel, c_imagersizes,
problem_selections, &problem_constants, calibration_object_spacing,
calibration_object_width_n, calibration_object_height_n, verbose, false);
std::vector<double> residuals = {c_x_final, c_x_final + Nmeasurements};
return std::make_unique<mrcal_result>(
true, intrinsics, stats.rms_reproj_error__pixels, residuals,
calobject_warp, stats.Noutliers_board);
}
struct MrcalSolveOptions {
// If true, we solve for the intrinsics core. Applies only to those models
// that HAVE a core (fx,fy,cx,cy)
int do_optimize_intrinsics_core;
// If true, solve for the non-core lens parameters
int do_optimize_intrinsics_distortions;
// If true, solve for the geometry of the cameras
int do_optimize_extrinsics;
// If true, solve for the poses of the calibration object
int do_optimize_frames;
// If true, optimize the shape of the calibration object
int do_optimize_calobject_warp;
// If true, apply the regularization terms in the solver
int do_apply_regularization;
// Whether to try to find NEW outliers. The outliers given on
// input are respected regardless
int do_apply_outlier_rejection;
};
// lifted from mrcal-pywrap.c. Restrict a given selection to only valid options
// License for mrcal-pywrap.c:
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
static mrcal_problem_selections_t
construct_problem_selections(MrcalSolveOptions s, int Ncameras_intrinsics,
int Ncameras_extrinsics, int Nframes,
int Nobservations_board) {
// By default we optimize everything we can
if (s.do_optimize_intrinsics_core < 0)
s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0;
if (s.do_optimize_intrinsics_distortions < 0)
s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0;
if (s.do_optimize_extrinsics < 0)
s.do_optimize_extrinsics = Ncameras_extrinsics > 0;
if (s.do_optimize_frames < 0)
s.do_optimize_frames = Nframes > 0;
if (s.do_optimize_calobject_warp < 0)
s.do_optimize_calobject_warp = Nobservations_board > 0;
return {
.do_optimize_intrinsics_core =
static_cast<bool>(s.do_optimize_intrinsics_core),
.do_optimize_intrinsics_distortions =
static_cast<bool>(s.do_optimize_intrinsics_distortions),
.do_optimize_extrinsics = static_cast<bool>(s.do_optimize_extrinsics),
.do_optimize_frames = static_cast<bool>(s.do_optimize_frames),
.do_optimize_calobject_warp =
static_cast<bool>(s.do_optimize_calobject_warp),
.do_apply_regularization = static_cast<bool>(s.do_apply_regularization),
.do_apply_outlier_rejection =
static_cast<bool>(s.do_apply_outlier_rejection),
// .do_apply_regularization_unity_cam01 = false
};
}
bool lensmodel_one_validate_args(mrcal_lensmodel_t *mrcal_lensmodel,
std::vector<double> intrinsics,
bool do_check_layout) {
int NlensParams = mrcal_lensmodel_num_params(mrcal_lensmodel);
int NlensParams_have = intrinsics.size();
if (NlensParams != NlensParams_have) {
BARF("intrinsics.shape[-1] MUST be %d. Instead got %d", NlensParams,
NlensParams_have);
return false;
}
return true;
}
mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool,
Size boardSize, Size imagerSize, double squareSize,
double focal_len_guess) {
using std::vector, std::runtime_error;
if (!c_observations_board_pool) {
throw runtime_error("board was null");
}
double fx = focal_len_guess;
double fy = fx;
double cx = (imagerSize.width / 2.0) - 0.5;
double cy = (imagerSize.height / 2.0) - 0.5;
vector<Point3f> objectPoints;
vector<Point2d> imagePoints;
// Fill in object/image points
for (int i = 0; i < boardSize.height; i++) {
for (int j = 0; j < boardSize.width; j++) {
auto &corner = c_observations_board_pool[i * boardSize.width + j];
// weight<0 means ignored -- filter these out
if (corner.z >= 0) {
imagePoints.emplace_back(corner.x, corner.y);
objectPoints.push_back(Point3f(j * squareSize, i * squareSize, 0));
}
}
}
{
// convert from stereographic to pinhole to match python
std::vector<mrcal_point2_t> mrcal_imagepts(imagePoints.size());
std::transform(
imagePoints.begin(), imagePoints.end(), mrcal_imagepts.begin(),
[](const auto &pt) { return mrcal_point2_t{.x = pt.x, .y = pt.y}; });
mrcal_lensmodel_t model{.type = MRCAL_LENSMODEL_STEREOGRAPHIC};
std::vector<mrcal_point3_t> out(imagePoints.size());
const double intrinsics[] = {fx, fy, cx, cy};
bool ret = mrcal_unproject(out.data(), mrcal_imagepts.data(),
mrcal_imagepts.size(), &model, intrinsics);
if (!ret) {
std::cerr << "couldn't unproject!" << std::endl;
}
model = {.type = MRCAL_LENSMODEL_PINHOLE};
mrcal_project(mrcal_imagepts.data(), NULL, NULL, out.data(), out.size(),
&model, intrinsics);
std::transform(mrcal_imagepts.begin(), mrcal_imagepts.end(),
imagePoints.begin(),
[](const auto &pt) { return Point2d{pt.x, pt.y}; });
}
// Initial guess at intrinsics
Mat cameraMatrix = (Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
Mat distCoeffs = Mat(4, 1, CV_64FC1, Scalar(0));
Mat_<double> rvec, tvec;
vector<Point3f> objectPoints3;
for (auto a : objectPoints)
objectPoints3.push_back(Point3f(a.x, a.y, 0));
// for (auto& o : objectPoints) std::cout << o << std::endl;
// for (auto& i : imagePoints) std::cout << i << std::endl;
// std::cout << "cam mat\n" << cameraMatrix << std::endl;
// std::cout << "distortion: " << distCoeffs << std::endl;
solvePnP(objectPoints3, imagePoints, cameraMatrix, distCoeffs, rvec, tvec,
false, SOLVEPNP_ITERATIVE);
return mrcal_pose_t{.r = {.x = rvec(0), .y = rvec(1), .z = rvec(2)},
.t = {.x = tvec(0), .y = tvec(1), .z = tvec(2)}};
}
mrcal_result::~mrcal_result() {
// cholmod_l_free_sparse(&Jt, cctx.cc);
return;
}
// Code taken from mrcal, license:
// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
std::unique_ptr<mrcal_result> mrcal_main(
// List, depth is ordered array observation[N frames, object_height,
// object_width] = [x,y, weight] weight<0 means ignored)
std::span<mrcal_point3_t> observations_board,
// RT transform from camera to object
std::span<mrcal_pose_t> frames_rt_toref,
// Chessboard size, in corners (not squares)
Size calobjectSize, double calibration_object_spacing,
// res, pixels
Size cameraRes, double focal_length_guess) {
std::unique_ptr<mrcal_result> result;
{
// stereographic initial guess for intrinsics
double cx = (cameraRes.width / 2.0) - 0.5;
double cy = (cameraRes.height / 2.0) - 0.5;
std::vector<double> intrinsics = {focal_length_guess, focal_length_guess,
cx, cy};
std::cout << "Initial solve (geometry only)" << std::endl;
mrcal_problem_selections_t options = construct_problem_selections(
{.do_optimize_intrinsics_core = false,
.do_optimize_intrinsics_distortions = false,
.do_optimize_extrinsics = false,
.do_optimize_frames = true,
.do_optimize_calobject_warp = false,
.do_apply_regularization = false,
.do_apply_outlier_rejection = false},
1, 0, frames_rt_toref.size(), frames_rt_toref.size());
mrcal_lensmodel_t mrcal_lensmodel;
mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC;
// And run calibration. This should mutate frames_rt_toref in place
result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize,
calibration_object_spacing, cameraRes, options,
mrcal_lensmodel, intrinsics);
}
{
std::cout
<< "Initial solve (geometry and LENSMODEL_STEREOGRAPHIC core only)"
<< std::endl;
mrcal_problem_selections_t options = construct_problem_selections(
{.do_optimize_intrinsics_core = true,
.do_optimize_intrinsics_distortions = true,
.do_optimize_extrinsics = false,
.do_optimize_frames = true,
.do_optimize_calobject_warp = false,
.do_apply_regularization = false,
.do_apply_outlier_rejection = false},
1, 0, frames_rt_toref.size(), frames_rt_toref.size());
mrcal_lensmodel_t mrcal_lensmodel;
mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC;
result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize,
calibration_object_spacing, cameraRes, options,
mrcal_lensmodel, result->intrinsics);
}
{
// Now we've got a seed, expand intrinsics to our target model
// see
// https://github.com/dkogan/mrcal/blob/33c3c50d5b7f991aca3a8e71ca52c5fffd153ef2/mrcal-calibrate-cameras#L533
mrcal_lensmodel_t mrcal_lensmodel;
mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
// num distortion params
int nparams = mrcal_lensmodel_num_params(&mrcal_lensmodel);
std::vector<double> intrinsics(nparams);
// copy core in
std::copy(result->intrinsics.begin(), result->intrinsics.end(),
intrinsics.begin());
// and generate random distortion
std::random_device rd;
std::mt19937 gen(rd());
std::uniform_real_distribution<> dis(-0.5, 0.5);
int nDistortion = nparams - 4;
std::vector<double> seedDistortions(nDistortion);
for (int j = 0; j < seedDistortions.size(); j++) {
if (j < 5)
seedDistortions[j] = dis(gen) * 2.0 * 1e-6;
else
seedDistortions[j] = dis(gen) * 2.0 * 1e-9;
}
// copy distortion into our big intrinsics array
std::copy(seedDistortions.begin(), seedDistortions.end(),
intrinsics.begin() + result->intrinsics.size());
std::cout
<< "Optimizing everything except board warp from seeded intrinsics"
<< std::endl;
mrcal_problem_selections_t options = construct_problem_selections(
{.do_optimize_intrinsics_core = true,
.do_optimize_intrinsics_distortions = true,
.do_optimize_extrinsics = true,
.do_optimize_frames = true,
.do_optimize_calobject_warp = false,
.do_apply_regularization = true,
.do_apply_outlier_rejection = true},
1, 0, frames_rt_toref.size(), frames_rt_toref.size());
result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize,
calibration_object_spacing, cameraRes, options,
mrcal_lensmodel, intrinsics);
}
{
std::cout << "Final, full solve" << std::endl;
mrcal_problem_selections_t options = construct_problem_selections(
{.do_optimize_intrinsics_core = true,
.do_optimize_intrinsics_distortions = true,
.do_optimize_extrinsics = true,
.do_optimize_frames = true,
.do_optimize_calobject_warp = true,
.do_apply_regularization = true,
.do_apply_outlier_rejection = true},
1, 0, frames_rt_toref.size(), frames_rt_toref.size());
mrcal_lensmodel_t mrcal_lensmodel;
mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize,
calibration_object_spacing, cameraRes, options,
mrcal_lensmodel, result->intrinsics);
}
return result;
}
bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat,
const cv::Mat *distCoeffs, CameraLensModel lensModel,
// Extra stuff for splined stereographic models
uint16_t order, uint16_t Nx, uint16_t Ny,
uint16_t fov_x_deg) {
mrcal_lensmodel_t mrcal_lensmodel;
switch (lensModel) {
case CameraLensModel::LENSMODEL_OPENCV5:
mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV5;
break;
case CameraLensModel::LENSMODEL_OPENCV8:
mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
break;
case CameraLensModel::LENSMODEL_STEREOGRAPHIC:
mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC;
break;
case CameraLensModel::LENSMODEL_SPLINED_STEREOGRAPHIC:
mrcal_lensmodel.type = MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC;
/* Maximum degree of each 1D polynomial. This is almost certainly 2 */
/* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */
mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order = order;
/* The horizontal field of view. Not including fov_y. It's proportional with
* Ny and Nx */
mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.fov_x_deg =
fov_x_deg;
/* We have a Nx by Ny grid of control points */
mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx = Nx;
mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny = Ny;
break;
default:
std::cerr << "Unknown lensmodel\n";
return false;
}
if (!(dst->cols == 2 && dst->cols == 2)) {
std::cerr << "Bad input array size\n";
return false;
}
if (!(dst->type() == CV_64FC2 && dst->type() == CV_64FC2)) {
std::cerr << "Bad input type -- need CV_64F\n";
return false;
}
if (!(dst->isContinuous() && dst->isContinuous())) {
std::cerr << "Bad input array -- need continuous\n";
return false;
}
// extract intrinsics core from opencv camera matrix
double fx = cameraMat->at<double>(0, 0);
double fy = cameraMat->at<double>(1, 1);
double cx = cameraMat->at<double>(0, 2);
double cy = cameraMat->at<double>(1, 2);
// Core, distortion coefficients concatenated
int NlensParams = mrcal_lensmodel_num_params(&mrcal_lensmodel);
std::vector<double> intrinsics(NlensParams);
intrinsics[0] = fx;
intrinsics[1] = fy;
intrinsics[2] = cx;
intrinsics[3] = cy;
for (size_t i = 0; i < distCoeffs->cols; i++) {
intrinsics[i + 4] = distCoeffs->at<double>(i);
}
// input points in the distorted image pixel coordinates
mrcal_point2_t *in = reinterpret_cast<mrcal_point2_t *>(dst->data);
// vec3 observation vectors defined up-to-length
std::vector<mrcal_point3_t> out(dst->rows);
mrcal_unproject(out.data(), in, dst->rows, &mrcal_lensmodel,
intrinsics.data());
// The output is defined "up-to-length"
// Let's project through pinhole again
// Output points in pinhole pixel coordinates
mrcal_point2_t *pinhole_pts = reinterpret_cast<mrcal_point2_t *>(dst->data);
size_t bound = dst->rows;
for (size_t i = 0; i < bound; i++) {
// from mrcal-project-internal/pinhole model
mrcal_point3_t &p = out[i];
double z_recip = 1. / p.z;
double x = p.x * z_recip;
double y = p.y * z_recip;
pinhole_pts[i].x = x * fx + cx;
pinhole_pts[i].y = y * fy + cy;
}
return true;
}

BIN
wpical/src/main/native/win/wpical.ico Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 20 KiB

1
wpical/src/main/native/win/wpical.rc Normal file
View File

@@ -0,0 +1 @@
IDI_ICON1 ICON "wpical.ico"

10
wpical/src/test/native/cpp/main.cpp Normal file
View File

@@ -0,0 +1,10 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <gtest/gtest.h>
int main(int argc, char** argv) {
::testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}

89
wpical/src/test/native/cpp/test_calibrate.cpp Normal file
View File

@@ -0,0 +1,89 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <cameracalibration.h>
#include <fieldcalibration.h>
#include <string>
#include <gtest/gtest.h>
#include <wpi/json.h>
const std::string projectRootPath = PROJECT_ROOT_PATH;
const std::string calSavePath =
projectRootPath.substr(0,
projectRootPath.find("/src/main/native/resources")) +
"/build";
cameracalibration::CameraModel cameraModel = {
.intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
.distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
.avg_reprojection_error = 0.0};
#ifdef __linux__
const std::string fileSuffix = ".avi";
const std::string videoLocation = "/altfieldvideo";
#else
const std::string fileSuffix = ".mp4";
const std::string videoLocation = "/fieldvideo";
#endif
TEST(Camera_CalibrationTest, OpenCV_Typical) {
int ret = cameracalibration::calibrate(
projectRootPath + "/testcalibration" + fileSuffix, cameraModel, 0.709f,
0.551f, 12, 8, false);
cameracalibration::dumpJson(cameraModel,
calSavePath + "/cameracalibration.json");
EXPECT_EQ(ret, 0);
}
TEST(Camera_CalibrationTest, OpenCV_Atypical) {
int ret = cameracalibration::calibrate(
projectRootPath + videoLocation + "/long" + fileSuffix, cameraModel,
0.709f, 0.551f, 12, 8, false);
EXPECT_EQ(ret, 1);
}
TEST(Camera_CalibrationTest, MRcal_Typical) {
int ret = cameracalibration::calibrate(
projectRootPath + "/testcalibration" + fileSuffix, cameraModel, .709f, 12,
8, 1080, 1920, false);
EXPECT_EQ(ret, 0);
}
TEST(Camera_CalibrationTest, MRcal_Atypical) {
int ret = cameracalibration::calibrate(
projectRootPath + videoLocation + "/short" + fileSuffix, cameraModel,
0.709f, 12, 8, 1080, 1920, false);
EXPECT_EQ(ret, 1);
}
TEST(Field_CalibrationTest, Typical) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", 3, false);
EXPECT_EQ(ret, 0);
}
TEST(Field_CalibrationTest, Atypical_Bad_Camera_Model_Directory) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
projectRootPath + videoLocation + "/long" + fileSuffix,
projectRootPath + "/2024-crescendo.json", 3, false);
EXPECT_EQ(ret, 1);
}
TEST(Field_CalibrationTest, Atypical_Bad_Ideal_JSON) {
int ret = fieldcalibration::calibrate(
projectRootPath + videoLocation, calSavePath,
calSavePath + "/cameracalibration.json",
calSavePath + "/cameracalibration.json", 3, false);
EXPECT_EQ(ret, 1);
}
TEST(Field_CalibrationTest, Atypical_Bad_Input_Directory) {
int ret = fieldcalibration::calibrate(
projectRootPath + "", calSavePath,
calSavePath + "/cameracalibration.json",
projectRootPath + "/2024-crescendo.json", 3, false);
EXPECT_EQ(ret, 1);
}

167
wpical/src/test/native/cpp/test_result_is_exact.cpp Normal file
View File

@@ -0,0 +1,167 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#include <chrono>
#include <cstdio>
#include <fstream>
#include <iostream>
#include <map>
#include <string>
#include <vector>
#include <gtest/gtest.h>
#include <mrcal_wrapper.h>
using namespace cv;
struct cmpByFilename {
bool operator()(const std::string& a, const std::string& b) const {
auto a2 = std::stoi(a.substr(3, a.length() - 7));
auto b2 = std::stoi(b.substr(3, b.length() - 7));
// std::cout << a2 << " _ " << b2 << std::endl;
return a2 < b2;
}
};
std::vector<double> calibrate(const std::string& fname, cv::Size boardSize,
cv::Size imagerSize) {
std::ifstream file(fname);
if (!file.is_open()) {
std::cerr << "Unable to open file corners.vnl" << std::endl;
return {};
}
std::string line;
std::map<std::string, std::vector<mrcal_point3_t>> points;
while (std::getline(file, line)) {
if (line.starts_with("#")) {
continue;
}
if (line.empty()) {
continue;
}
try {
std::istringstream iss(line);
std::string filename, x_str, y_str, level_str;
iss >> filename >> x_str >> y_str >> level_str;
using std::stod;
// From calibration.py:
// if weight_column_kind == 'level': the 4th column is a decimation level
// of the
// detected corner. If we needed to cut down the image resolution to
// detect a corner, its coordinates are known less precisely, and we use
// that information to weight the errors appropriately later. We set the
// output weight = 1/2^level. If the 4th column is '-' or <0, the given
// point was not detected, and should be ignored: we set weight = -1
double level;
if (level_str == "-") {
level = -1;
} else {
level = stod(level_str);
}
double weight;
if (level < 0) {
weight = -1;
} else {
weight = std::pow(0.5, level);
}
points[filename].push_back(
mrcal_point3_t{stod(x_str), stod(y_str), weight});
// std::printf("put %s\n", *name);
} catch (std::exception const& e) {
std::perror(e.what());
}
}
file.close();
auto start = std::chrono::steady_clock::now();
std::vector<mrcal_point3_t> observations_board;
std::vector<mrcal_pose_t> frames_rt_toref;
// Pre-allocate worst case
size_t total_points = boardSize.width * boardSize.height * points.size();
observations_board.reserve(total_points);
frames_rt_toref.reserve(points.size());
std::chrono::steady_clock::time_point begin =
std::chrono::steady_clock::now();
for (const auto& [key, value] : points) {
if (value.size()) {
auto ret = getSeedPose(value.data(), boardSize, imagerSize, 0.03, 1200);
// Append to the Big List of board corners/levels
observations_board.insert(observations_board.end(), value.begin(),
value.end());
// And list of pose seeds
frames_rt_toref.push_back(ret);
} else {
std::printf("No points for %s\n", key.c_str());
}
}
std::chrono::steady_clock::time_point end = std::chrono::steady_clock::now();
std::cout << "Seed took: "
<< std::chrono::duration_cast<std::chrono::milliseconds>(end -
begin)
.count()
<< "[ms]" << std::endl;
auto cal_result = mrcal_main(observations_board, frames_rt_toref, boardSize,
0.030, imagerSize, 1200);
auto dt = std::chrono::steady_clock::now() - start;
int dt_ms = dt.count();
auto& stats = *cal_result;
double max_error =
*std::max_element(stats.residuals.begin(), stats.residuals.end());
if (1) {
std::printf("\n===============================\n\n");
std::printf("RMS Reprojection Error: %.2f pixels\n", stats.rms_error);
std::printf("Worst residual (by measurement): %.1f pixels\n", max_error);
std::printf("Noutliers: %i of %zu (%.1f percent of the data)\n",
stats.Noutliers_board, total_points,
100.0 * stats.Noutliers_board / total_points);
std::printf("calobject_warp: [%f, %f]\n", stats.calobject_warp.x2,
stats.calobject_warp.y2);
std::printf("dt, seeding + solve: %f ms\n", dt_ms / 1e6);
std::printf("Intrinsics [%zu]: ", stats.intrinsics.size());
for (auto i : stats.intrinsics)
std::printf("%f ", i);
std::printf("\n");
}
return stats.intrinsics;
}
const std::string projectRootPath = PROJECT_ROOT_PATH;
TEST(MrcalResultExactlyMatchesTest, lifecam_1280) {
auto calculated_intrinsics{calibrate(
projectRootPath + "/lifecam_1280p_10x10.vnl", {10, 10}, {1280, 720})};
// ## generated with mrgingham --jobs 4 --gridn 10
// /home/mmorley/photonvision/test-resources/calibrationSquaresImg/lifecam/2024-01-02_lifecam_1280/*.png
// # generated on 2024-12-01 11:51:55 with /usr/bin/mrcal-calibrate-cameras
// --corners-cache corners.vnl --lensmodel LENSMODEL_OPENCV8 --focal 1200
// --object-spacing 0.0254 --object-width-n 10 '*.png'
std::vector<double> mrcal_cli_groundtruth_intrinsics{
1130.892767, 1129.149453, 609.9417222, 349.6457279,
0.1489878273, -1.348622726, 0.002839630852, 0.001135629909,
2.560627057, -0.03170208336, 0.0695788644, -0.09547554864};
for (int i = 0; i < 12; i++) {
EXPECT_NEAR(mrcal_cli_groundtruth_intrinsics[i], calculated_intrinsics[i],
1e-6);
}
}