diff --git a/upstream_utils/mrcal.py b/upstream_utils/mrcal.py index 40a8e826d65..2781a83948f 100644 --- a/upstream_utils/mrcal.py +++ b/upstream_utils/mrcal.py @@ -3,7 +3,7 @@ import os import shutil -from upstream_utils import Lib, walk_cwd_and_copy_if +from upstream_utils import Lib, comment_out_invalid_includes, walk_cwd_and_copy_if def copy_upstream_src(wpilib_root): @@ -16,14 +16,23 @@ def copy_upstream_src(wpilib_root): ]: shutil.rmtree(os.path.join(wpical, d), ignore_errors=True) - walk_cwd_and_copy_if( - lambda dp, f: (f.endswith(".h") or f.endswith(".hh")) + files = walk_cwd_and_copy_if( + lambda dp, f: f.endswith(".h") + and not f.endswith("mrcal-image.h") and not f.endswith("stereo.h") and not f.endswith("stereo-matching-libelas.h") and not dp.startswith(os.path.join(".", "test")), os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), ) - walk_cwd_and_copy_if( + for f in files: + comment_out_invalid_includes( + f, + [ + os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), + os.path.join(wpical, "src/main/native/thirdparty/mrcal/generated"), + ], + ) + files = walk_cwd_and_copy_if( lambda dp, f: (f.endswith(".c") or f.endswith(".cc") or f.endswith(".pl")) and not f.endswith("mrcal-pywrap.c") and not f.endswith("image.c") @@ -34,6 +43,14 @@ def copy_upstream_src(wpilib_root): and not dp.startswith(os.path.join(".", "test")), os.path.join(wpical, "src/main/native/thirdparty/mrcal/src"), ) + for f in files: + comment_out_invalid_includes( + f, + [ + os.path.join(wpical, "src/main/native/thirdparty/mrcal/include"), + os.path.join(wpical, "src/main/native/thirdparty/mrcal/generated"), + ], + ) def main(): diff --git a/wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh b/wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh deleted file mode 100644 index b6f217354c9..00000000000 --- a/wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh +++ /dev/null @@ -1,582 +0,0 @@ -// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. -// Government sponsorship acknowledged. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 - -#pragma once - -/* - Automatic differentiation routines. Used in poseutils-uses-autodiff.cc. See - that file for usage examples - */ - -// Apparently I need this in MSVC to get constants -#define _USE_MATH_DEFINES -#include -#include -#include "strides.h" - -template struct vec_withgrad_t; - - -template -struct val_withgrad_t -{ - double x; - double j[NGRAD]; - - __attribute__ ((visibility ("hidden"))) - val_withgrad_t(double _x = 0.0) : x(_x) - { - for(int i=0; i operator+( const val_withgrad_t& b ) const - { - val_withgrad_t y = *this; - y.x += b.x; - for(int i=0; i operator+( double b ) const - { - val_withgrad_t y = *this; - y.x += b; - return y; - } - __attribute__ ((visibility ("hidden"))) - void operator+=( const val_withgrad_t& b ) - { - *this = (*this) + b; - } - __attribute__ ((visibility ("hidden"))) - val_withgrad_t operator-( const val_withgrad_t& b ) const - { - val_withgrad_t y = *this; - y.x -= b.x; - for(int i=0; i operator-( double b ) const - { - val_withgrad_t y = *this; - y.x -= b; - return y; - } - __attribute__ ((visibility ("hidden"))) - void operator-=( const val_withgrad_t& b ) - { - *this = (*this) - b; - } - __attribute__ ((visibility ("hidden"))) - val_withgrad_t operator-() const - { - return (*this) * (-1); - } - __attribute__ ((visibility ("hidden"))) - val_withgrad_t operator*( const val_withgrad_t& b ) const - { - val_withgrad_t y; - y.x = x * b.x; - for(int i=0; i operator*( double b ) const - { - val_withgrad_t y; - y.x = x * b; - for(int i=0; i& b ) - { - *this = (*this) * b; - } - __attribute__ ((visibility ("hidden"))) - void operator*=( const double b ) - { - *this = (*this) * b; - } - __attribute__ ((visibility ("hidden"))) - val_withgrad_t operator/( const val_withgrad_t& b ) const - { - val_withgrad_t y; - y.x = x / b.x; - for(int i=0; i operator/( double b ) const - { - return (*this) * (1./b); - } - __attribute__ ((visibility ("hidden"))) - void operator/=( const val_withgrad_t& b ) - { - *this = (*this) / b; - } - __attribute__ ((visibility ("hidden"))) - void operator/=( const double b ) - { - *this = (*this) / b; - } - __attribute__ ((visibility ("hidden"))) - val_withgrad_t sqrt(void) const - { - val_withgrad_t y; - y.x = ::sqrt(x); - for(int i=0; i square(void) const - { - val_withgrad_t s; - s.x = x*x; - for(int i=0; i sin(void) const - { - const double s = ::sin(x); - const double c = ::cos(x); - val_withgrad_t y; - y.x = s; - for(int i=0; i cos(void) const - { - const double s = ::sin(x); - const double c = ::cos(x); - val_withgrad_t y; - y.x = c; - for(int i=0; i sincos(void) const - { - const double s = ::sin(x); - const double c = ::cos(x); - vec_withgrad_t sc; - sc.v[0].x = s; - sc.v[1].x = c; - for(int i=0; i tan(void) const - { - const double s = ::sin(x); - const double c = ::cos(x); - val_withgrad_t y; - y.x = s/c; - for(int i=0; i atan2(val_withgrad_t& x) const - { - val_withgrad_t th; - const val_withgrad_t& y = *this; - - th.x = ::atan2(y.x, x.x); - // dth/dv = d/dv atan2(y,x) - // = d/dv atan(y/x) - // = 1 / (1 + y^2/x^2) d/dv (y/x) - // = x^2 / (x^2 + y^2) / x^2 * (dy/dv x - y dx/dv) - // = 1 / (x^2 + y^2) * (dy/dv x - y dx/dv) - double norm2 = y.x*y.x + x.x*x.x; - for(int i=0; i asin(void) const - { - val_withgrad_t th; - th.x = ::asin(x); - double dasin_dx = 1. / ::sqrt( 1. - x*x ); - for(int i=0; i acos(void) const - { - val_withgrad_t th; - th.x = ::acos(x); - double dacos_dx = -1. / ::sqrt( 1. - x*x ); - for(int i=0; i sinx_over_x(// To avoid recomputing it - const val_withgrad_t& sinx) const - { - // For small x I need special-case logic. In the limit as x->0 I have - // sin(x)/x -> 1. But I'm propagating gradients, so I need to capture - // that. I have - // - // d(sin(x)/x)/dx = - // (x cos(x) - sin(x))/x^2 - // - // As x -> 0 this is - // - // (cos(x) - x sin(x) - cos(x)) / (2x) = - // (- x sin(x)) / (2x) = - // -sin(x) / 2 = - // 0 - // - // So for small x the gradient is 0 - if(fabs(x) < 1e-5) - return val_withgrad_t(1.0); - - return sinx / (*this); - } -}; - - -template -struct vec_withgrad_t -{ - val_withgrad_t v[NVEC]; - - vec_withgrad_t() {} - - __attribute__ ((visibility ("hidden"))) - void init_vars(const double* x_in, int ivar0, int Nvars, int i_gradvec0 = -1, - int stride = sizeof(double)) - { - // Initializes vector entries ivar0..ivar0+Nvars-1 inclusive using the - // data in x_in[]. x_in[0] corresponds to vector entry ivar0. If - // i_gradvec0 >= 0 then vector ivar0 corresponds to gradient index - // i_gradvec0, with all subsequent entries being filled-in - // consecutively. It's very possible that NGRAD > Nvars. Initially the - // subset of the gradient array corresponding to variables - // i_gradvec0..i_gradvec0+Nvars-1 is an identity, with the rest being 0 - memset((char*)&v[ivar0], 0, Nvars*sizeof(v[0])); - for(int i=ivar0; i= 0) - v[i].j[i_gradvec0+i-ivar0] = 1.0; - } - } - - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t(const double* x_in, int i_gradvec0 = -1, - int stride = sizeof(double)) - { - init_vars(x_in, 0, NVEC, i_gradvec0, stride); - } - - __attribute__ ((visibility ("hidden"))) - val_withgrad_t& operator[](int i) - { - return v[i]; - } - - __attribute__ ((visibility ("hidden"))) - const val_withgrad_t& operator[](int i) const - { - return v[i]; - } - - __attribute__ ((visibility ("hidden"))) - void operator+=( const vec_withgrad_t& x ) - { - (*this) = (*this) + x; - } - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t operator+( const vec_withgrad_t& x ) const - { - vec_withgrad_t p; - for(int i=0; i& x ) - { - (*this) = (*this) + x; - } - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t operator+( const val_withgrad_t& x ) const - { - vec_withgrad_t p; - for(int i=0; i operator+( double x ) const - { - vec_withgrad_t p; - for(int i=0; i& x ) - { - (*this) = (*this) - x; - } - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t operator-( const vec_withgrad_t& x ) const - { - vec_withgrad_t p; - for(int i=0; i& x ) - { - (*this) = (*this) - x; - } - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t operator-( const val_withgrad_t& x ) const - { - vec_withgrad_t p; - for(int i=0; i operator-( double x ) const - { - vec_withgrad_t p; - for(int i=0; i& x ) - { - (*this) = (*this) * x; - } - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t operator*( const vec_withgrad_t& x ) const - { - vec_withgrad_t p; - for(int i=0; i& x ) - { - (*this) = (*this) * x; - } - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t operator*( const val_withgrad_t& x ) const - { - vec_withgrad_t p; - for(int i=0; i operator*( double x ) const - { - vec_withgrad_t p; - for(int i=0; i& x ) - { - (*this) = (*this) / x; - } - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t operator/( const vec_withgrad_t& x ) const - { - vec_withgrad_t p; - for(int i=0; i& x ) - { - (*this) = (*this) / x; - } - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t operator/( const val_withgrad_t& x ) const - { - vec_withgrad_t p; - for(int i=0; i operator/( double x ) const - { - vec_withgrad_t p; - for(int i=0; i dot( const vec_withgrad_t& x) const - { - val_withgrad_t d; // initializes to 0 - for(int i=0; i e = x.v[i]*v[i]; - d += e; - } - return d; - } - - __attribute__ ((visibility ("hidden"))) - vec_withgrad_t cross( const vec_withgrad_t& x) const - { - vec_withgrad_t c; - c[0] = v[1]*x.v[2] - v[2]*x.v[1]; - c[1] = v[2]*x.v[0] - v[0]*x.v[2]; - c[2] = v[0]*x.v[1] - v[1]*x.v[0]; - return c; - } - - __attribute__ ((visibility ("hidden"))) - val_withgrad_t norm2(void) const - { - return dot(*this); - } - - __attribute__ ((visibility ("hidden"))) - val_withgrad_t mag(void) const - { - val_withgrad_t l2 = norm2(); - return l2.sqrt(); - } - - __attribute__ ((visibility ("hidden"))) - void extract_value(double* out, - int stride = sizeof(double), - int ivar0 = 0, int Nvars = NVEC) const - { - for(int i=ivar0; i -static -vec_withgrad_t -cross( const vec_withgrad_t& a, - const vec_withgrad_t& b ) -{ - vec_withgrad_t c; - c.v[0] = a.v[1]*b.v[2] - a.v[2]*b.v[1]; - c.v[1] = a.v[2]*b.v[0] - a.v[0]*b.v[2]; - c.v[2] = a.v[0]*b.v[1] - a.v[1]*b.v[0]; - return c; -} - -template -static -val_withgrad_t -cross_norm2( const vec_withgrad_t& a, - const vec_withgrad_t& b ) -{ - vec_withgrad_t c = cross(a,b); - return c.norm2(); -} - -template -static -val_withgrad_t -cross_mag( const vec_withgrad_t& a, - const vec_withgrad_t& b ) -{ - vec_withgrad_t c = cross(a,b); - return c.mag(); -} diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h deleted file mode 100644 index 086810d4c69..00000000000 --- a/wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h +++ /dev/null @@ -1,124 +0,0 @@ -// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S. -// Government sponsorship acknowledged. All rights reserved. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 - -#pragma once - -// mrcal images. These are completely uninteresting, and don't do anything -// better that other image read/write APIS. If you have image libraries running, -// use those. If not, the ones defined here should be light and painless - -// I support several image types: -// - "uint8": 8-bit grayscale -// - "uint16": 16-bit grayscale (using the system endian-ness) -// - "bgr": 24-bit BGR color -// -// Each type defines several functions in the MRCAL_IMAGE_DECLARE() macro: -// -// - mrcal_image_TYPE_t container image -// - mrcal_image_TYPE_at(mrcal_image_TYPE_t* image, int x, int y) -// - mrcal_image_TYPE_at_const(const mrcal_image_TYPE_t* image, int x, int y) -// - mrcal_image_TYPE_t mrcal_image_TYPE_crop(mrcal_image_TYPE_t* image, in x0, int y0, int w, int h) -// - mrcal_image_TYPE_save (const char* filename, const mrcal_image_TYPE_t* image); -// - mrcal_image_TYPE_load( mrcal_image_TYPE_t* image, const char* filename); -// -// The image-loading functions require a few notes: -// -// An image structure to fill in is given. image->data will be allocated to the -// proper size. It is the caller's responsibility to free(image->data) when -// they're done. Usage sample: -// -// mrcal_image_uint8_t image; -// mrcal_image_uint8_load(&image, image_filename); -// .... do stuff ... -// free(image.data); -// -// mrcal_image_uint8_load() converts images to 8-bpp grayscale. Color and -// palettized images are accepted -// -// mrcal_image_uint16_load() does NOT convert images. The images being read must -// already be stored as 16bpp grayscale images -// -// mrcal_image_bgr_load() converts images to 24-bpp color - -#include -#include - - -typedef struct { uint8_t bgr[3]; } mrcal_bgr_t; - -#define MRCAL_IMAGE_DECLARE(T, Tname) \ -typedef struct \ -{ \ - union \ - { \ - /* in pixels */ \ - struct {int w, h;}; \ - struct {int width, height;}; \ - struct {int cols, rows;}; \ - }; \ - int stride; /* in bytes */ \ - T* data; \ -} mrcal_image_ ## Tname ## _t; \ - \ -static inline \ -T* mrcal_image_ ## Tname ## _at(mrcal_image_ ## Tname ## _t* image, int x, int y) \ -{ \ - return &image->data[x + y*image->stride / sizeof(T)]; \ -} \ - \ -static inline \ -const T* mrcal_image_ ## Tname ## _at_const(const mrcal_image_ ## Tname ## _t* image, int x, int y) \ -{ \ - return &image->data[x + y*image->stride / sizeof(T)]; \ -} \ - \ -static inline \ -mrcal_image_ ## Tname ## _t \ -mrcal_image_ ## Tname ## _crop(mrcal_image_ ## Tname ## _t* image, \ - int x0, int y0, \ - int w, int h) \ -{ \ - return (mrcal_image_ ## Tname ## _t){ .w = w, \ - .h = h, \ - .stride = image->stride, \ - .data = mrcal_image_ ## Tname ## _at(image,x0,y0) }; \ -} - -#define MRCAL_IMAGE_SAVE_LOAD_DECLARE(T, Tname) \ -bool mrcal_image_ ## Tname ## _save (const char* filename, const mrcal_image_ ## Tname ## _t* image); \ -bool mrcal_image_ ## Tname ## _load( mrcal_image_ ## Tname ## _t* image, const char* filename); - - -// Common images types -MRCAL_IMAGE_DECLARE(uint8_t, uint8); -MRCAL_IMAGE_DECLARE(uint16_t, uint16); -MRCAL_IMAGE_DECLARE(mrcal_bgr_t, bgr); -MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint8_t, uint8); -MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint16_t, uint16); -MRCAL_IMAGE_SAVE_LOAD_DECLARE(mrcal_bgr_t, bgr); - -// Uncommon types. Not everything supports these -MRCAL_IMAGE_DECLARE(int8_t, int8); -MRCAL_IMAGE_DECLARE(int16_t, int16); - -MRCAL_IMAGE_DECLARE(int32_t, int32); -MRCAL_IMAGE_DECLARE(uint32_t, uint32); -MRCAL_IMAGE_DECLARE(int64_t, int64); -MRCAL_IMAGE_DECLARE(uint64_t, uint64); - -MRCAL_IMAGE_DECLARE(float, float); -MRCAL_IMAGE_DECLARE(double, double); - -// Load the image into whatever type is stored on disk -bool mrcal_image_anytype_load(// output - // This is ONE of the known types - mrcal_image_uint8_t* image, - int* bits_per_pixel, - int* channels, - // input - const char* filename); diff --git a/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h b/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h index ebb751eca30..32cf141d5cd 100644 --- a/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h +++ b/wpical/src/main/native/thirdparty/mrcal/include/mrcal.h @@ -13,9 +13,9 @@ #include "mrcal-types.h" #include "poseutils.h" -#include "stereo.h" +// #include "stereo.h" #include "triangulation.h" -#include "mrcal-image.h" +// #include "mrcal-image.h" //////////////////////////////////////////////////////////////////////////////// //////////////////// Lens models diff --git a/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp b/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp index c4ba29b1022..4bf2df8951d 100644 --- a/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp +++ b/wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp @@ -9,7 +9,7 @@ #include #include -#include "autodiff.hh" +// #include "autodiff.hh" extern "C" { #include "cahvore.h" diff --git a/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp index deb4670887e..cf636663721 100644 --- a/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp +++ b/wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp @@ -6,7 +6,7 @@ // // http://www.apache.org/licenses/LICENSE-2.0 -#include "autodiff.hh" +// #include "autodiff.hh" #include "strides.h" extern "C" { diff --git a/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp b/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp index a6807455b94..7de15b44f44 100644 --- a/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp +++ b/wpical/src/main/native/thirdparty/mrcal/src/triangulation.cpp @@ -6,7 +6,7 @@ // // http://www.apache.org/licenses/LICENSE-2.0 -#include "autodiff.hh" +// #include "autodiff.hh" extern "C" { #include "triangulation.h"