pax_global_header 0000666 0000000 0000000 00000000064 15123677724 0014530 g ustar 00root root 0000000 0000000 52 comment=c311b0acdb29d3f6c1a5abeaf17dc6a7e2ab10d9
mrcal-2.5/ 0000775 0000000 0000000 00000000000 15123677724 0012474 5 ustar 00root root 0000000 0000000 mrcal-2.5/.gitattributes 0000664 0000000 0000000 00000000000 15123677724 0015355 0 ustar 00root root 0000000 0000000 mrcal-2.5/.gitignore 0000664 0000000 0000000 00000000544 15123677724 0014467 0 ustar 00root root 0000000 0000000 *~
*.d
*.o
*.so*
*.docstring.h
cscope*
*.pyc
*.orig
*.i
*.rej
*generated*
*GENERATED*
core*
debian/*.log
debian/*.substvars
debian/*-stamp
debian/*.debhelper
debian/files
debian/*/
*.pickle
test/test-gradients
test/test-lensmodel-string-manipulation
test/test-cahvor
test/test-parser-cameramodel
*.pod
*GENERATED.*
*.backup*
*.fig.bak
ltximg/
doc/out
mrcal-2.5/CHOLMOD_factorization.docstring 0000664 0000000 0000000 00000005404 15123677724 0020436 0 ustar 00root root 0000000 0000000 A basic Python interface to CHOLMOD
SYNOPSIS
from scipy.sparse import csr_matrix
indptr = np.array([0, 2, 3, 6, 8])
indices = np.array([0, 2, 2, 0, 1, 2, 1, 2])
data = np.array([1, 2, 3, 4, 5, 6, 7, 8], dtype=float)
Jsparse = csr_matrix((data, indices, indptr))
Jdense = Jsparse.toarray()
print(Jdense)
===> [[1. 0. 2.]
[0. 0. 3.]
[4. 5. 6.]
[0. 7. 8.]]
bt = np.array(((1., 5., 3.), (2., -2., -8)))
print(nps.transpose(bt))
===> [[ 1. 2.]
[ 5. -2.]
[ 3. -8.]]
F = mrcal.CHOLMOD_factorization(Jsparse)
xt = F.solve_xt_JtJ_bt(bt)
print(nps.transpose(xt))
===> [[ 0.02199662 0.33953751]
[ 0.31725888 0.46982516]
[-0.21996616 -0.50648618]]
print(nps.matmult(nps.transpose(Jdense), Jdense, nps.transpose(xt)))
===> [[ 1. 2.]
[ 5. -2.]
[ 3. -8.]]
The core of the mrcal optimizer is a sparse linear least squares solver using
CHOLMOD to solve a large, sparse linear system. CHOLMOD is a C library, but it
is sometimes useful to invoke it from Python.
The CHOLMOD_factorization class factors a matrix JtJ, and this method uses that
factorization to efficiently solve the linear equation JtJ x = b. The usual
linear algebra conventions refer to column vectors, but numpy generally deals
with row vectors, so I talk about solving the equivalent transposed problem: xt
JtJ = bt. The difference is purely notational.
The class takes a sparse array J as an argument in __init__(). J is optional,
but there's no way in Python to pass it later, so from Python you should always
pass J. This is optional for internal initialization from C code.
J must be given as an instance of scipy.sparse.csr_matrix. csr is a row-major
sparse representation. CHOLMOD wants column-major matrices, so it see this
matrix J as a transpose: the CHOLMOD documentation refers to this as "At". And
the CHOLMOD documentation talks about factoring AAt, while I talk about
factoring JtJ. These are the same thing.
The factorization of JtJ happens in __init__(), and we use this factorization
later (as many times as we want) to solve JtJ x = b by calling
solve_xt_JtJ_bt().
This class carefully checks its input for validity, but makes no effort to be
flexible: anything that doesn't look right will result in an exception.
Specifically:
- J.data, J.indices, J.indptr must all be numpy arrays
- J.data, J.indices, J.indptr must all have exactly one dimension
- J.data, J.indices, J.indptr must all be C-contiguous (the normal numpy order)
- J.data must hold 64-bit floating-point values (dtype=float)
- J.indices, J.indptr must hold 32-bit integers (dtype=np.int32)
ARGUMENTS
The __init__() function takes
- J: a sparse array in a scipy.sparse.csr_matrix object
mrcal-2.5/CHOLMOD_factorization_rcond.docstring 0000664 0000000 0000000 00000001506 15123677724 0021622 0 ustar 00root root 0000000 0000000 Compute rough estimate of reciprocal of condition number
SYNOPSIS
b, x, J, factorization = \
mrcal.optimizer_callback(**optimization_inputs)
rcond = factorization.rcond()
Calls cholmod_rcond(). Its documentation says:
Returns a rough estimate of the reciprocal of the condition number: the
minimum entry on the diagonal of L (or absolute entry of D for an LDLT
factorization) divided by the maximum entry. L can be real, complex, or
zomplex. Returns -1 on error, 0 if the matrix is singular or has a zero or NaN
entry on the diagonal of L, 1 if the matrix is 0-by-0, or
min(diag(L))/max(diag(L)) otherwise. Never returns NaN; if L has a NaN on the
diagonal it returns zero instead.
ARGUMENTS
- None
RETURNED VALUE
A single floating point value: an estimate of the reciprocal of the condition
number
mrcal-2.5/CHOLMOD_factorization_solve_xt_JtJ_bt.docstring 0000664 0000000 0000000 00000006061 15123677724 0023615 0 ustar 00root root 0000000 0000000 Solves the linear system JtJ x = b using CHOLMOD
SYNOPSIS
from scipy.sparse import csr_matrix
indptr = np.array([0, 2, 3, 6, 8])
indices = np.array([0, 2, 2, 0, 1, 2, 1, 2])
data = np.array([1, 2, 3, 4, 5, 6, 7, 8], dtype=float)
Jsparse = csr_matrix((data, indices, indptr))
Jdense = Jsparse.toarray()
print(Jdense)
===> [[1. 0. 2.]
[0. 0. 3.]
[4. 5. 6.]
[0. 7. 8.]]
bt = np.array(((1., 5., 3.), (2., -2., -8)))
print(nps.transpose(bt))
===> [[ 1. 2.]
[ 5. -2.]
[ 3. -8.]]
F = mrcal.CHOLMOD_factorization(Jsparse)
xt = F.solve_xt_JtJ_bt(bt)
print(nps.transpose(xt))
===> [[ 0.02199662 0.33953751]
[ 0.31725888 0.46982516]
[-0.21996616 -0.50648618]]
print(nps.matmult(nps.transpose(Jdense), Jdense, nps.transpose(xt)))
===> [[ 1. 2.]
[ 5. -2.]
[ 3. -8.]]
The core of the mrcal optimizer is a sparse linear least squares solver using
CHOLMOD to solve a large, sparse linear system. CHOLMOD is a C library, but it
is sometimes useful to invoke it from Python.
The CHOLMOD_factorization class factors a matrix JtJ, and this method uses that
factorization to efficiently solve the linear equation JtJ x = b. The usual
linear algebra conventions refer to column vectors, but numpy generally deals
with row vectors, so I talk about solving the equivalent transposed problem: xt
JtJ = bt. The difference is purely notational.
As many vectors b as we'd like may be given at one time (in rows of bt). The
dimensions of the returned array xt will match the dimensions of the given array
bt.
Broadcasting is supported: any leading dimensions will be processed correctly,
as long as bt has shape (..., Nstate)
This function carefully checks its input for validity, but makes no effort to be
flexible: anything that doesn't look right will result in an exception.
Specifically:
- bt must be C-contiguous (the normal numpy order)
- bt must contain 64-bit floating-point values (dtype=float)
This function is now able to pass different values of "sys" to the internal
cholmod_solve2() call. This is specified with the "mode" argument. By default,
we use CHOLMOD_A, which is the default behavior: we solve JtJ x = b. All the
other modes supported by CHOLMOD are supported. From cholmod.h:
CHOLMOD_A: solve Ax=b
CHOLMOD_LDLt: solve LDL'x=b
CHOLMOD_LD: solve LDx=b
CHOLMOD_DLt: solve DL'x=b
CHOLMOD_L: solve Lx=b
CHOLMOD_Lt: solve L'x=b
CHOLMOD_D: solve Dx=b
CHOLMOD_P: permute x=Px
CHOLMOD_Pt: permute x=P'x
See the CHOLMOD documentation and source for details.
ARGUMENTS
- bt: a numpy array of shape (..., Nstate). This array must be C-contiguous and
it must have dtype=float
- sys: optional string, defaulting to "A": solve JtJ x = b. Selects the specific
problem being solved; see the description above. The value passed to "sys"
should be the string with or without the "CHOLMOD_" prefix
RETURNED VALUE
The transpose of the solution array x, in a numpy array of the same shape as the
input bt
mrcal-2.5/LICENSE 0000664 0000000 0000000 00000026274 15123677724 0013514 0 ustar 00root root 0000000 0000000 Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
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 not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
mrcal-2.5/Makefile 0000664 0000000 0000000 00000016466 15123677724 0014151 0 ustar 00root root 0000000 0000000 include choose_mrbuild.mk
include $(MRBUILD_MK)/Makefile.common.header
# "0" or undefined means "false"
# everything else means "true"
# libelas stereo matcher. Available in Debian/non-free. I don't want to depend
# on anything in non-free, so I default to not using libelas
USE_LIBELAS ?= 0
# convert all USE_XXX:=0 to an empty string
$(foreach v,$(filter USE_%,$(.VARIABLES)),$(if $(filter 0,${$v}),$(eval undefine $v)))
# to print them all: $(foreach v,$(filter USE_%,$(.VARIABLES)),$(warning $v = '${$v}'))
PROJECT_NAME := mrcal
ABI_VERSION := 5
TAIL_VERSION := 0
VERSION = $(VERSION_FROM_PROJECT)
LIB_SOURCES += \
mrcal.c \
opencv.c \
uncertainty.c \
image.c \
stereo.c \
poseutils.c \
poseutils-opencv.c \
poseutils-uses-autodiff.cc \
triangulation.cc \
cahvore.cc \
traverse-sensor-links.c \
heap.cc \
python-cameramodel-converter.c
# This is a utility function for external Python wrapping. I link this into
# libmrcal.so, but libmrcal.so does NOT link with libpython. 99% of the usage of
# libmrcal.so will not use this, so it should work without libpython. People
# using this function will be doing so as part of PyArg_ParseTupleAndKeywords(),
# so they will be linking to libpython anyway. Thus I weaken all the references
# to libpython here. c_build_rule is the default logic in mrbuild
#
# This is a DEEP rabbithole. With Debian/trixie (released summer 2025) objcopy
# --weaken works to weaken the symbols after compiling. With older objcopy (not
# sure which), the objcopy command completes successfully, but it doesn't
# actually weaken the symbols. I thus run nm to find the non-weakened symbols;
# if any remain, I recompile, explicitly setting __attribute__((weak)) on these
# symbols. I can't do this on the first pass because before compiling even once
# I don't know which Py symbols I'm going to get (some are generated by the
# Python internals).
#
# Finally, the second-pass-recompile-with-attribute-weak doesn't work with
# clang: the references produced by inline functions in Python.h that *I* am not
# calling are not weakened. I catch this case here, and throw an error. In the
# very near future, few people are going to have the too-old binutils, and we'll
# be done
python-cameramodel-converter.o: %.o:%.c
$(c_build_rule) && mv $@ _$@
$(OBJCOPY) --wildcard --weaken-symbol='Py*' --weaken-symbol='_Py*' _$@ $@ && mv $@ _$@
$(NM) --undef _$@ | awk '$$1 == "U" && $$2 ~ "Py" { print $$2 }' > python-cameramodel-converter-py-symbol-refs
if [ -s python-cameramodel-converter-py-symbol-refs ]; then \
< python-cameramodel-converter-py-symbol-refs \
awk 'BEGIN {ORS=""; print "#define PY_REFS(_) " } {print "_("$$1") "} ' \
> python-cameramodel-converter-py-symbol-refs.h \
&& \
$(c_build_rule) -DWEAKEN_PY_REFS; \
if $(NM) --undef $@ | grep -E -q '\sU\s+_?Py'; then \
echo "ERROR: Strong symbols remain! See the Makefile for notes"; \
false; \
fi \
else \
mv _$@ $@; \
fi
EXTRA_CLEAN += \
python-cameramodel-converter-py-symbol-refs \
python-cameramodel-converter-py-symbol-refs.h \
_python-cameramodel-converter.o
ifneq (${USE_LIBELAS},) # using libelas
LIB_SOURCES := $(LIB_SOURCES) stereo-matching-libelas.cc
endif
BIN_SOURCES += \
test/test-gradients.c \
test/test-cahvor.c \
test/test-lensmodel-string-manipulation.c \
test/test-parser-cameramodel.c \
test/test-heap.c
LDLIBS += -ldogleg -lstb -lpng -ljpeg -llapack
ifneq (${USE_LIBELAS},) # using libelas
LDLIBS += -lelas
endif
CFLAGS += --std=gnu99
CCXXFLAGS += -Wno-missing-field-initializers -Wno-unused-variable -Wno-unused-parameter -Wno-missing-braces
$(patsubst %.c,%.o,$(shell grep -l '#include .*minimath\.h' *.c */*.c)): minimath/minimath_generated.h
minimath/minimath_generated.h: minimath/minimath_generate.pl
./$< > $@.tmp && mv $@.tmp $@
EXTRA_CLEAN += minimath/minimath_generated.h
DIST_INCLUDE += \
mrcal.h \
image.h \
internal.h \
basic-geometry.h \
poseutils.h \
triangulation.h \
types.h \
stereo.h \
heap.h \
python-cameramodel-converter.h
DIST_BIN := \
mrcal-calibrate-cameras \
mrcal-convert-lensmodel \
mrcal-show-distortion-off-pinhole \
mrcal-show-splined-model-correction \
mrcal-show-projection-uncertainty \
mrcal-show-projection-diff \
mrcal-reproject-points \
mrcal-reproject-image \
mrcal-graft-models \
mrcal-to-cahvor \
mrcal-from-cahvor \
mrcal-to-kalibr \
mrcal-from-kalibr \
mrcal-from-ros \
mrcal-show-geometry \
mrcal-show-valid-intrinsics-region \
mrcal-is-within-valid-intrinsics-region \
mrcal-triangulate \
mrcal-cull-corners \
mrcal-show-residuals-board-observation \
mrcal-show-residuals \
mrcal-stereo
# generate manpages from distributed binaries, and ship them. This is a hoaky
# hack because apparenly manpages from python tools is a crazy thing to want to
# do
DIST_MAN := $(addsuffix .1,$(DIST_BIN))
# if using an older mrbuild SO won't be defined, and we need it
SO ?= so
# parser
cameramodel-parser_GENERATED.c: cameramodel-parser.re mrcal.h
re2c $< > $@.tmp && mv $@.tmp $@
LIB_SOURCES += cameramodel-parser_GENERATED.c
EXTRA_CLEAN += cameramodel-parser_GENERATED.c
cameramodel-parser_GENERATED.o: CCXXFLAGS += -fno-fast-math
ALL_NPSP_EXTENSION_MODULES := $(patsubst %-genpywrap.py,%,$(wildcard *-genpywrap.py))
ifeq (${USE_LIBELAS},) # not using libelas
ALL_NPSP_EXTENSION_MODULES := $(filter-out elas,$(ALL_NPSP_EXTENSION_MODULES))
endif
ALL_PY_EXTENSION_MODULES := _mrcal $(patsubst %,_%_npsp,$(ALL_NPSP_EXTENSION_MODULES))
%/:
mkdir -p $@
######### python stuff
%-npsp-pywrap-GENERATED.c: %-genpywrap.py
python3 $< > $@.tmp && mv $@.tmp $@
mrcal/_%_npsp$(PY_EXT_SUFFIX): %-npsp-pywrap-GENERATED.o libmrcal.$(SO) libmrcal.$(SO).${ABI_VERSION}
$(PY_MRBUILD_LINKER) $(PY_MRBUILD_LDFLAGS) $(LDFLAGS) $< -lmrcal -o $@
ALL_NPSP_C := $(patsubst %,%-npsp-pywrap-GENERATED.c,$(ALL_NPSP_EXTENSION_MODULES))
ALL_NPSP_O := $(patsubst %,%-npsp-pywrap-GENERATED.o,$(ALL_NPSP_EXTENSION_MODULES))
ALL_NPSP_SO := $(patsubst %,mrcal/_%_npsp$(PY_EXT_SUFFIX),$(ALL_NPSP_EXTENSION_MODULES))
EXTRA_CLEAN += $(ALL_NPSP_C)
# https://gcc.gnu.org/bugzilla/show_bug.cgi?id=95635
$(ALL_NPSP_O): CFLAGS += -Wno-array-bounds
mrcal-pywrap.o: $(addsuffix .h,$(wildcard *.docstring))
mrcal/_mrcal$(PY_EXT_SUFFIX): mrcal-pywrap.o libmrcal.$(SO) libmrcal.$(SO).${ABI_VERSION}
$(PY_MRBUILD_LINKER) $(PY_MRBUILD_LDFLAGS) $(LDFLAGS) $< -lmrcal -lsuitesparseconfig -o $@
CFLAGS += -I/usr/include/suitesparse
PYTHON_OBJECTS := mrcal-pywrap.o python-cameramodel-converter.o $(ALL_NPSP_O)
$(PYTHON_OBJECTS): CFLAGS += $(PY_MRBUILD_CFLAGS)
# The python libraries (compiled ones and ones written in python) all live in
# mrcal/
DIST_PY3_MODULES := mrcal
all: mrcal/_mrcal$(PY_EXT_SUFFIX) $(ALL_NPSP_SO)
EXTRA_CLEAN += mrcal/*.$(SO)
TESTS_ALL_TARGETS := test-all test-nosampling test-triangulation-uncertainty test-external-data
$(TESTS_ALL_TARGETS): all
./test.sh $@
.PHONY: $(TESTS_ALL_TARGETS)
test:
@echo "Which test set should we run? I know about '$(TESTS_ALL_TARGETS)'" > /dev/stderr; false
.PHONY: test
include Makefile.doc
include $(MRBUILD_MK)/Makefile.common.footer
# to work with mrbuild < 1.14
OBJCOPY ?= objcopy
mrcal-2.5/Makefile.doc 0000664 0000000 0000000 00000012746 15123677724 0014712 0 ustar 00root root 0000000 0000000 # -*- Makefile -*-
# To use the latest tag in the documentation, run
#
# VERSION_USE_LATEST_TAG=1 make
## I generate a single mrcal-python-api-reference.html for ALL the Python code.
## This is large
DOC_HTML += doc/out/mrcal-python-api-reference.html
doc/out/mrcal-python-api-reference.html: $(wildcard mrcal/*.py) $(patsubst %,mrcal/%$(PY_EXT_SUFFIX),$(ALL_PY_EXTENSION_MODULES)) libmrcal.so.$(ABI_VERSION) | doc/out/
python3 doc/pydoc.py -w mrcal > $@.tmp && mv $@.tmp $@
DOC_ALL_FIG := $(wildcard doc/*.fig)
DOC_ALL_SVG_FROM_FIG := $(patsubst doc/%.fig,doc/out/figures/%.svg,$(DOC_ALL_FIG))
DOC_ALL_PDF_FROM_FIG := $(patsubst doc/%.fig,doc/out/figures/%.pdf,$(DOC_ALL_FIG))
doc: $(DOC_ALL_SVG_FROM_FIG) $(DOC_ALL_PDF_FROM_FIG)
$(DOC_ALL_SVG_FROM_FIG): doc/out/figures/%.svg: doc/%.fig | doc/out/figures/
fig2dev -L svg $< $@
$(DOC_ALL_PDF_FROM_FIG): doc/out/figures/%.pdf: doc/%.fig | doc/out/figures/
fig2dev -L pdf $< $@
DOC_ALL_DOT := $(wildcard doc/*.dot)
DOC_ALL_TEX_FROM_DOT := $(patsubst doc/%.dot,doc/out/figures/%.tex,$(DOC_ALL_DOT))
DOC_ALL_PDF_FROM_DOT := $(patsubst %.tex,%.pdf,$(DOC_ALL_TEX_FROM_DOT))
doc: $(DOC_ALL_PDF_FROM_DOT)
# This is very error-prone for some reason. The below options and the existing
# doc/cross-uncertainty.dot work, but tweaking either makes it now work.
$(DOC_ALL_TEX_FROM_DOT): doc/out/figures/%.tex: doc/%.dot | doc/out/figures/
dot2tex --autosize -c -t math $< -o $@
# pdflatex ignores the directory of the given argument, so I need to cd in there
# first
%.pdf: %.tex
cd $(dir $@); pdflatex $(notdir $<)
## I'd like to place docs for each Python submodule in a separate .html (instead
## of a single huge mrcal-python-api-reference.html). Here's an attempt at at
## that. This works, but needs more effort:
##
## - top level mrcal.html is confused about what it contains. It has all of
## _mrcal and _poseutils for some reason
## - cross-submodule links don't work
#
# doc-reference: \
# $(patsubst mrcal/%.py,doc/mrcal.%.html,$(filter-out %/__init__.py,$(wildcard mrcal/*.py))) \
# $(patsubst %,doc/out/mrcal.%.html,$(ALL_PY_EXTENSION_MODULES)) \
# doc/out/mrcal.html
# doc/out/mrcal.%.html: \
# mrcal/%.py \
# $(patsubst %,mrcal/%$(PY_EXT_SUFFIX),$(ALL_PY_EXTENSION_MODULES)) \
# libmrcal.so.$(ABI_VERSION)
# doc/pydoc.py -w mrcal.$* > $@.tmp && mv $@.tmp $@
# doc/out/mrcal.%.html: mrcal/%$(PY_EXT_SUFFIX)
# doc/pydoc.py -w mrcal.$* > $@.tmp && mv $@.tmp $@
# doc/out/mrcal.html: \
# $(wildcard mrcal/*.py) \
# $(patsubst %,mrcal/%$(PY_EXT_SUFFIX),$(ALL_PY_EXTENSION_MODULES)) \
# libmrcal.so.$(ABI_VERSION)
# doc/pydoc.py -w mrcal > $@.tmp && mv $@.tmp $@
# .PHONY: doc-reference
DOC_ALL_CSS := $(wildcard doc/*.css)
DOC_ALL_CSS_TARGET := $(patsubst doc/%,doc/out/%,$(DOC_ALL_CSS))
doc: $(DOC_ALL_CSS_TARGET)
$(DOC_ALL_CSS_TARGET): doc/out/%.css: doc/%.css | doc/out/
cp $< doc/out
DOC_ALL_ORG := $(wildcard doc/*.org)
DOC_ALL_HTML_TARGET := $(patsubst doc/%.org,doc/out/%.html,$(DOC_ALL_ORG))
DOC_HTML += $(DOC_ALL_HTML_TARGET)
# This ONE command creates ALL the html files, so I want a pattern rule to indicate
# that. I want to do:
# %/out/a.html %/out/b.html %/out/c.html: %/a.org %/b.org %/c.org
$(addprefix %,$(patsubst doc/%,/%,$(DOC_ALL_HTML_TARGET))): $(addprefix %,$(patsubst doc/%,/%,$(DOC_ALL_ORG)))
emacs --chdir=doc -l mrcal-docs-publish.el --batch --eval '(load-library "org")' --eval '(org-publish-all t nil)'
$(DOC_ALL_HTML_TARGET): doc/mrcal-docs-publish.el | doc/out/
$(DIST_MAN): %.1: %.pod
pod2man --center="mrcal: camera projection, calibration toolkit" --name=MRCAL --release="mrcal $(VERSION)" --section=1 $< $@
%.pod: %
$(MRBUILD_BIN)/make-pod-from-help $< > $@.tmp && cat footer.pod >> $@.tmp && mv $@.tmp $@
EXTRA_CLEAN += $(DIST_MAN) $(patsubst %.1,%.pod,$(DIST_MAN))
# I generate a manpage. Some perl stuff to add the html preamble
MANPAGES_HTML := $(patsubst %,doc/out/%.html,$(DIST_BIN))
doc/out/%.html: %.pod | doc/out/
pod2html --noindex --css=mrcal.css --infile=$< | \
perl -ne 'BEGIN {$$h = `cat doc/mrcal-preamble-GENERATED.html`;} if(!/(.*
)(.*)/s) { print; } else { print "$$1 $$h $$2"; }' > $@.tmp && mv $@.tmp $@
DOC_HTML += $(MANPAGES_HTML)
$(DOC_HTML): doc/mrcal-preamble-GENERATED.html
# If the git HEAD moves, I regenerate the preamble. It contains a version string
# that uses the git info. This still isn't complete. A new git tag SHOULD
# trigger this to be regenerated, but it doesn't. I'll do that later
doc/mrcal-preamble-GENERATED.html: doc/mrcal-preamble-TEMPLATE.html $(and $(wildcard .git),.git/$(shell cut -d' ' -f2 .git/HEAD))
< $< sed s/@@VERSION@@/$(VERSION)/g > $@.tmp && mv $@.tmp $@
EXTRA_CLEAN += doc/mrcal-preamble-GENERATED.html
# Documentation uses the latest tag only to denote the version. This will only
# work properly after a 'make clean' (If you "make", everything is built without
# VERSION_USE_LATEST_TAG=1; then when you "make doc", the products built without
# VERSION_USE_LATEST_TAG=1 are used). Beware
doc: VERSION_USE_LATEST_TAG=1
doc: $(DOC_HTML) | doc/out/external
.PHONY: doc
doc/out/external:
ln -fs ../../../mrcal-doc-external $@
# the whole output documentation directory
EXTRA_CLEAN += doc/out
# Convenience rules. Don't blindly use these if you aren't Dima and you aren't
# using his machine
publish-doc: doc
rsync --exclude '*~' --exclude external -avu doc/out/ mrcal.secretsauce.net:/var/www/mrcal/docs-latest-release
publish-doc-external:
rsync -avu ../mrcal-doc-external/ mrcal.secretsauce.net:/var/www/mrcal/docs-2.3/external
.PHONY: publish-doc publish-doc-external
mrcal-2.5/README.org 0000664 0000000 0000000 00000002262 15123677724 0014144 0 ustar 00root root 0000000 0000000 * SYNOPSIS
#+BEGIN_EXAMPLE
$ mrcal-calibrate-cameras --focal 2000
--outdir /tmp --object-spacing 0.01
--object-width-n 10 '/tmp/left*.png' '/tmp/right*.png'
... lots of output as the solve runs ...
Wrote /tmp/camera0-0.cameramodel
Wrote /tmp/camera0-1.cameramodel
#+END_EXAMPLE
And now we have a calibration!
* SUMMARY
=mrcal= is a generic toolkit built to solve the calibration and SFM-like
problems we encounter at NASA/JPL. Functionality related to these problems is
exposed as a set of C and Python libraries and some commandline tools.
* DESCRIPTION
Extensive documentation is available at https://mrcal.secretsauce.net/
* INSTALLATION
Install and/or building instructions are on the [[https://mrcal.secretsauce.net/install.html]["Building or installing" page]].
* REPOSITORY
https://www.github.com/dkogan/mrcal/
* AUTHOR
Dima Kogan (=dima@secretsauce.net=)
* LICENSE AND COPYRIGHT
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
mrcal-2.5/_autodiff.hh 0000664 0000000 0000000 00000040255 15123677724 0014763 0 ustar 00root root 0000000 0000000 // 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();
}
mrcal-2.5/_cahvore.h 0000664 0000000 0000000 00000001717 15123677724 0014441 0 ustar 00root root 0000000 0000000 // 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
#ifdef __cplusplus
extern "C" {
#endif
#include
#include "basic-geometry.h"
bool project_cahvore_internals( // outputs
mrcal_point3_t* __restrict pdistorted,
double* __restrict dpdistorted_dintrinsics_nocore,
double* __restrict dpdistorted_dp,
// inputs
const mrcal_point3_t* __restrict p,
const double* __restrict intrinsics_nocore,
const double cahvore_linearity);
#ifdef __cplusplus
}
#endif
mrcal-2.5/_rectification_maps.docstring 0000664 0000000 0000000 00000000266 15123677724 0020420 0 ustar 00root root 0000000 0000000 Construct image transformation maps to make rectified images
This is an internal function. You probably want mrcal.rectification_maps(). See
the docs for that function for details.
mrcal-2.5/_rectified_resolution.docstring 0000664 0000000 0000000 00000000266 15123677724 0020776 0 ustar 00root root 0000000 0000000 Compute the resolution to be used for the rectified system
This is an internal function. You probably want mrcal.rectified_resolution(). See the
docs for that function for details.
mrcal-2.5/_rectified_system.docstring 0000664 0000000 0000000 00000000247 15123677724 0020116 0 ustar 00root root 0000000 0000000 Build rectified models for stereo rectification
This is an internal function. You probably want mrcal.rectified_system(). See the
docs for that function for details.
mrcal-2.5/_strides.h 0000664 0000000 0000000 00000003617 15123677724 0014470 0 ustar 00root root 0000000 0000000 // 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
#ifdef __cplusplus
extern "C" {
#endif
// 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
#ifdef __cplusplus
}
#endif
mrcal-2.5/_util.h 0000664 0000000 0000000 00000000772 15123677724 0013767 0 ustar 00root root 0000000 0000000 // 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
#ifdef __cplusplus
extern "C" {
#endif
#include
#define MSG(fmt, ...) fprintf(stderr, "%s(%d): " fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__)
#ifdef __cplusplus
}
#endif
mrcal-2.5/analyses/ 0000775 0000000 0000000 00000000000 15123677724 0014313 5 ustar 00root root 0000000 0000000 mrcal-2.5/analyses/calibrate-camera-imu.py 0000775 0000000 0000000 00000027760 15123677724 0020650 0 ustar 00root root 0000000 0000000 #!/usr/bin/python3
r'''A simple camera-IMU alignment method
This routine takes measurements from rigid rig containing an IMU and some
cameras, and compuites their relative geometry. The cameras are assumed to have
pre-calibrated intrinsics, which are currently trusted 100% by this tool
(weighing by uncertainty would be an improvement). The IMU is assumed to be
self-consistent, and we only use the measured gravity vector. This could be
easily extended into a more complex routine that computes the IMU biases and
such. We compute the relative translation and orientation of all the cameras,
and only an orientation from the IMU.
To gather the data, we place a stationary chessboard somewhere in the scene.
Then we collect a small number of datasets by placing the camera+IMU rig in
various orientation, where some set of cameras can observes the chessboard. We
gather chessboard images and and gravity measurements from the IMU. This allows
us to compute a set of poses to make all the measurements self-consistent.
This is a bit simplistic and could be improved, but does work quite well.
Reference coordinate system:
- camera0
Unknowns:
- rt_cam_cam0[i] for each camera i (6*(Ncameras-1) DOF)
- rt_cam0_board[j] at each time j (6*Nsnapshots DOF)
- r_imu_cam0 (3 DOF)
- g_board (2 DOF)
Measurements:
- q_board[i,j]
- g_imu[j]
Cost function for each i,j:
- Reprojection error q_board[i,j] - project(rt_cam_cam0[i] rt_cam0_board[j] p_board)
- r_imu_cam0 r_cam0_board[j] g_board - g_imu[j]
'''
import sys
import numpy as np
import numpysane as nps
import gnuplotlib as gp
import mrcal
import glob
import vnlog
import os
import re
import scipy
# Write the results here
Dout = '/tmp'
Din = '/tmp'
cameras = ('cam0', 'cam1', 'cam2',)
models = [mrcal.cameramodel(f"{cam}.cameramodel") \
for cam in cameras]
# The data is in vnl tables with the given columns. Each table is in a separate
# file, with the date/time ("YYYY-MM-DD-HH-MM-SS") in the filename identifying
# the instant in time when that data was captured. Sensor readings with the same
# tag can be assumed to have been gathered at the same instant in time. The INS
# data ends in "-gravity", the chessboard observation files have the camera name
# at the end of the filename instead.
dtype_gravity = np.dtype( [('acc0 acc1 acc2', float,(3,))])
dtype_corners = np.dtype( [('x y', float,(2,))])
tag_regex = '(20[0-9][0-9]-[0-9][0-9]-[0-9][0-9]-[0-9][0-9]-[0-9][0-9]-[0-9][0-9])'
glob_files_gravity = f'{Din}/*-gravity.vnl'
glob_files_corners = [f'{Din}/*-{cam}.vnl' for cam in cameras]
# chessboard
gridn = 14
object_spacing = 0.0588
# relative scale of pixels errors to IMU axis errors. These should match the
# uncertainty of each respective measurement
imu_err_scale__pixels_per_m_s2 = 0.3/0.05
files_gravity = sorted(glob.glob(glob_files_gravity))
files_corners_allcams = [sorted(glob.glob(g)) for g in glob_files_corners]
def tag_from_file(f):
m = re.search(tag_regex,f)
return m.group(1)
_isnapshot_from_tag = dict()
for i,f in enumerate(files_gravity):
_isnapshot_from_tag[ tag_from_file(f) ] = i
def isnapshot_from_file(f):
return _isnapshot_from_tag[tag_from_file(f)]
Nsnapshots = len(files_gravity)
g_imu = np.zeros( (Nsnapshots,3), dtype=float)
for f in files_gravity:
g = vnlog.slurp(f, dtype=dtype_gravity)['acc0 acc1 acc2']
g_imu[isnapshot_from_file(f)] = g
g_imu_mag = nps.mag(g_imu)
gunit_imu = g_imu / nps.dummy( g_imu_mag, -1)
Ncameras = len(cameras)
icam_from_cam = dict()
for i,c in enumerate(cameras):
icam_from_cam[c] = i
Nobservations = sum(len(files_corners) for files_corners in files_corners_allcams)
# qx,qy will be filled in later; weight=1 is hard-coded here now
observations_qxqyw = np.ones ( (Nobservations,gridn,gridn,3), dtype=float)
indices_frame_camera = np.zeros( (Nobservations,2), dtype=np.int32 )
i = 0
for icam in range(Ncameras):
for f in files_corners_allcams[icam]:
isnapshot = isnapshot_from_file(f)
observations_qxqyw[i,...,:2] = vnlog.slurp(f, dtype=dtype_corners)['x y'].reshape((gridn,gridn,2))
indices_frame_camera[i,0] = isnapshot
indices_frame_camera[i,1] = icam
i += 1
######## Seed
# I need to estimate each of the state:
# - rt_cam_cam0[i] for each camera i (6*(Ncameras-1) DOF)
# - rt_cam0_board[j] at each time j (6*Nsnapshots DOF)
# - r_imu_cam0 (3 DOF)
# - g_board (2 DOF)
# For each observation, estimate the cam-board transform. This assumes a nearby
# pinhole model, so it's a rough estimate. I take arbitrary subsets of these to
# get rt_cam_cam0 and rt_cam0_board
Rt_cam_board_all = \
mrcal.estimate_monocular_calobject_poses_Rt_tocam( indices_frame_camera,
observations_qxqyw,
object_spacing,
models)
rt_cam_cam0 = np.zeros( (Ncameras-1, 6), dtype=float)
rt_cam0_board = np.zeros( (Nsnapshots,6), dtype=float)
Nrt_cam_cam0_have = 0
Nrt_cam0_board_have = 0
# I go through to accumulate my seed.
#### WARNING: THIS DOES NOT WORK IN GENERAL
#### HERE I'M ASSUMING THAT CAMERA0 IS IN EVERY SNAPSHOT
for iobservation in range(Nobservations):
Rt_cam_board = Rt_cam_board_all[iobservation]
isnapshot,icam = indices_frame_camera[iobservation]
if icam==0 and not np.any(rt_cam0_board[isnapshot]):
rt_cam0_board[isnapshot] = mrcal.rt_from_Rt(Rt_cam_board)
Nrt_cam0_board_have += 1
if Nrt_cam0_board_have == len(rt_cam0_board):
break
if Nrt_cam0_board_have != len(rt_cam0_board):
raise Exception("ERROR: did not init all of rt_cam0_board")
for iobservation in range(Nobservations):
Rt_cam_board = Rt_cam_board_all[iobservation]
isnapshot,icam = indices_frame_camera[iobservation]
if icam!=0 and not np.any(rt_cam_cam0[icam-1]):
rt_cam_cam0[icam-1] = \
mrcal.compose_rt(mrcal.rt_from_Rt(Rt_cam_board),
rt_cam0_board[isnapshot],
inverted1 = True)
Nrt_cam_cam0_have += 1
if Nrt_cam_cam0_have == len(rt_cam_cam0):
break
if Nrt_cam_cam0_have != len(rt_cam_cam0):
raise Exception("ERROR: did not init all of rt_cam_cam0")
######## Solve
def unpack_state(b, have_imu = False):
Nstate_cameras = (Ncameras-1 + Nsnapshots) * 6
bcameras = b[:Nstate_cameras].reshape( (Ncameras-1 + Nsnapshots, 6) )
rt_cam_cam0 = bcameras[:Ncameras-1,:]
rt_cam0_board = bcameras[Ncameras-1:,:]
if not have_imu:
return rt_cam_cam0,rt_cam0_board
bimu = b[Nstate_cameras:]
r_imu_cam0 = bimu[:3]
a,b = bimu[3:]
gunit_board = np.array(( np.sin(a),
np.cos(a)*np.cos(b),
np.cos(a)*np.sin(b),))
return rt_cam_cam0,rt_cam0_board,r_imu_cam0,gunit_board
def pack_state(rt_cam_cam0, rt_cam0_board,
r_imu_cam0 = None,
gunit_board = None):
if r_imu_cam0 is None:
return \
nps.cat( *rt_cam_cam0, *rt_cam0_board ).ravel()
# have_imu
a = np.arcsin( gunit_board[0])
c = np.cos(a)
if np.abs(c) < 1e-8:
b = 0
else:
b = np.arctan2(gunit_board[2]/c,
gunit_board[1]/c,)
return \
nps.glue( rt_cam_cam0.ravel(),
rt_cam0_board.ravel(),
r_imu_cam0.ravel(),
a,
b,
axis = -1)
def optimizer_callback_camera(b, have_imu = False, debug = False):
if not have_imu:
rt_cam_cam0,rt_cam0_board = unpack_state(b, have_imu = have_imu)
else:
rt_cam_cam0,rt_cam0_board,r_imu_cam0,gunit_board = unpack_state(b, have_imu = have_imu)
x_corners = np.zeros((Nobservations,gridn,gridn,2), dtype=float)
for iobservation in range(Nobservations):
isnapshot,icam = indices_frame_camera[iobservation]
q_observed = observations_qxqyw[iobservation, ..., :2]
pcam = mrcal.transform_point_rt( rt_cam0_board[isnapshot],
calobject )
if icam > 0:
pcam = mrcal.transform_point_rt( rt_cam_cam0[icam-1],
pcam )
q = mrcal.project(pcam, *models[icam].intrinsics())
x_corners[iobservation,...] = q - q_observed
if debug:
print(f"Camera solve callback RMS error: {np.sqrt(np.mean(nps.norm2(x_corners).ravel()))}")
if not have_imu:
return x_corners.ravel()
gunit_cam0 = mrcal.rotate_point_r(rt_cam0_board[:,:3], gunit_board)
gunit_imu_solve = mrcal.rotate_point_r(r_imu_cam0, gunit_cam0)
g_imu_solve = gunit_imu_solve * 9.8
x_imu = g_imu_solve - g_imu
if debug:
print(f"IMU vector error: {x_imu}m/s^2")
return nps.glue(x_corners.ravel(),
x_imu.ravel() * imu_err_scale__pixels_per_m_s2,
axis = -1)
###### I solve the camera stuff by itself initially, since that should fit very
###### well
calobject = mrcal.ref_calibration_object(gridn, gridn,
object_spacing)
# state vector at the seed
b0 = pack_state(rt_cam_cam0, rt_cam0_board)
result = scipy.optimize.least_squares(optimizer_callback_camera, b0,
kwargs=dict(have_imu = False))
# if True:
# print("At the solution:")
# optimizer_callback_camera(result['x'], debug = True)
# mrcal.show_geometry( nps.glue( mrcal.identity_rt(),
# rt_cam_cam0,
# axis = -2),
# wait = True)
###### Now do a bigger optimization, including the IMU stuff
rt_cam_cam0,rt_cam0_board = unpack_state(result['x'])
####### For now I assume the board is sitting roughly vertically, and seed off
####### that. THIS IS NOT TRUE IN GENERAL
gunit_board = np.array((0., -1., 0.))
gunit_cam0 = mrcal.rotate_point_r(rt_cam0_board[:,:3], gunit_board)
R_imu_cam0 = mrcal.align_procrustes_vectors_R01(gunit_imu, gunit_cam0)
r_imu_cam0 = mrcal.r_from_R(R_imu_cam0)
gunit_imu_seed = mrcal.rotate_point_R(R_imu_cam0, gunit_cam0)
th_err_imu_seed = np.arccos(nps.inner(gunit_imu_seed, gunit_imu))
if False:
# should be "small"
print(th_err_imu_seed)
b0 = pack_state(rt_cam_cam0, rt_cam0_board, r_imu_cam0, gunit_board)
result = scipy.optimize.least_squares(optimizer_callback_camera, b0,
kwargs=dict(have_imu = True))
rt_cam_cam0,rt_cam0_board,r_imu_cam0,gunit_board = unpack_state(result['x'], have_imu = True)
rt_cam_cam0_mounted = \
nps.glue( mrcal.identity_rt(),
rt_cam_cam0,
axis = -2)
rt_imu_cam0 = nps.glue( r_imu_cam0, np.zeros((3,)),
axis = -1)
for icam in range(Ncameras):
cam = cameras[icam]
rt_cam_cam0_this = rt_cam_cam0_mounted[icam]
filename = f"{Dout}/{cam}-final.cameramodel"
m = mrcal.cameramodel(models[icam])
m.rt_cam_ref( rt_cam_cam0_this )
m.write(filename)
print(f"Wrote '{filename}'")
# And a dummy imu model; for visualization only
filename = f"{Dout}/imu-final.cameramodel"
m = mrcal.cameramodel(rt_cam_ref = rt_imu_cam0,
# dummy
intrinsics = ('LENSMODEL_PINHOLE', np.array((1,1,0,0))),
imagersize = (1,1) )
m.write(filename)
print(f"Wrote '{filename}'")
if True:
print("At the solution:")
optimizer_callback_camera(result['x'],
have_imu = True,
debug = True)
gunit_cam0 = mrcal.rotate_point_r(r_imu_cam0, gunit_imu, inverted=True)
gunit_board = mrcal.rotate_point_r(rt_cam0_board[:,:3], gunit_cam0, inverted=True)
print(f"guinit_board=\n{gunit_board}")
mrcal.show_geometry( nps.glue( rt_cam_cam0_mounted,
rt_imu_cam0,
axis = -2),
cameranames = cameras + ['imu'],
wait = True)
mrcal-2.5/analyses/dancing/ 0000775 0000000 0000000 00000000000 15123677724 0015716 5 ustar 00root root 0000000 0000000 mrcal-2.5/analyses/dancing/dance-study.py 0000775 0000000 0000000 00000176644 15123677724 0020535 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Simulates different chessboard dances to find the best technique
We want the shortest chessboard dances that produce the most confident results.
In a perfect world would put the chessboard in all the locations where we would
expect to use the visual system, since the best confidence is obtained in
regions where the chessboard was observed.
However, due to geometric constraints it is sometimes impossible to put the
board in the right locations. This tool clearly shows that filling the field of
view produces best results. But very wide lenses require huge chessboards,
displayed very close to the lens in order to fill the field of view. This means
that using a wide lens to look out to infinity will always result in potentially
too little projection confidence. This tool is intended to find the kind of
chessboard dance to get good confidences by simulating different geometries and
dances.
We arrange --Ncameras cameras horizontally, with an identity rotation, evenly
spaced with a spacing of --camera-spacing meters. The left camera is at the
origin.
We show the cameras lots of dense chessboards ("dense" meaning that every camera
sees all the points of all the chessboards). The chessboard come in two
clusters: "near" and "far". Each cluster is centered straight ahead of the
midpoint of all the cameras, with some random noise on the position and
orientation. The distances from the center of the cameras to the center of the
clusters are given by --range. This tool solves the calibration problem, and
generates uncertainty-vs-range curves. Each run of this tool generates a family
of this curve, for different values of Nframes-far, the numbers of chessboard
observations in the "far" cluster.
This tool scans some parameter (selected by --scan), and reports the
uncertainty-vs-range for the different values of this parameter (as a plot and
as an output vnl).
If we don't want to scan any parameter, and just want a single
uncertainty-vs-range plot, don't pass --scan.
'''
import sys
import argparse
import re
import os
def parse_args():
def positive_float(string):
try:
value = float(string)
except:
raise argparse.ArgumentTypeError("argument MUST be a positive floating-point number. Got '{}'".format(string))
if value <= 0:
raise argparse.ArgumentTypeError("argument MUST be a positive floating-point number. Got '{}'".format(string))
return value
def positive_int(string):
try:
value = int(string)
except:
raise argparse.ArgumentTypeError("argument MUST be a positive integer. Got '{}'".format(string))
if value <= 0 or abs(value-float(string)) > 1e-6:
raise argparse.ArgumentTypeError("argument MUST be a positive integer. Got '{}'".format(string))
return value
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--icam-uncertainty',
default = 0,
type=int,
help='''Which camera to use for the uncertainty reporting. I use the left-most one
(camera 0) by default''')
parser.add_argument('--camera-spacing',
default = 0.3,
type=float,
help='How many meters between adjacent cameras in our synthetic world')
parser.add_argument('--object-spacing',
default="0.077",
type=str,
help='''Width of each square in the calibration board, in meters. If --scan
object_width_n, this is used as the spacing for the
SMALLEST width. As I change object_width_n, I adjust
object_spacing accordingly, to keep the total board size
constant. If --scan object_spacing, this is MIN,MAX to
specify the bounds of the scan. In that case
object_width_n stays constant, so the board changes
size''')
parser.add_argument('--object-width-n',
type=str,
help='''How many points the calibration board has per horizontal side. If both are
omitted, we default the width and height to 10 (both
must be specified to use a non-default value). If --scan
object_width_n, this is MIN,MAX to specify the bounds of
the scan. In that case I assume a square object, and
ignore --object-height-n. Scanning object-width-n keeps
the board size constant''')
parser.add_argument('--object-height-n',
type=int,
help='''How many points the calibration board has per vertical side. If both are
omitted, we default the width and height to 10 (both
must be specified to use a non-default value). If --scan
object_width_n, this is ignored, and set equivalent to
the object width''')
parser.add_argument('--observed-pixel-uncertainty',
type=positive_float,
default = 1.0,
help='''The standard deviation of x and y pixel coordinates of the input observations
I generate. The distribution of the inputs is gaussian,
with the standard deviation specified by this argument.
Note: this is the x and y standard deviation, treated
independently. If each of these is s, then the LENGTH of
the deviation of each pixel is a Rayleigh distribution
with expected value s*sqrt(pi/2) ~ s*1.25''')
parser.add_argument('--lensmodel',
required=False,
type=str,
help='''Which lens model to use for the simulation. If omitted, we use the model
given on the commandline. We may want to use a
parametric model to generate data (model on the
commandline), but a richer splined model to solve''')
parser.add_argument('--skip-calobject-warp-solve',
action='store_true',
default=False,
help='''By default we assume the calibration target is
slightly deformed, and we compute this deformation. If
we want to assume that the chessboard shape is fixed,
pass this option. The actual shape of the board is given
by --calobject-warp''')
parser.add_argument('--calobject-warp',
type=float,
nargs=2,
default=(0.002, -0.005),
help='''The "calibration-object warp". These specify the
flex of the chessboard. By default, the board is
slightly warped (as is usually the case in real life).
To use a perfectly flat board, specify "0 0" here''')
parser.add_argument('--show-geometry-first-solve',
action = 'store_true',
help='''If given, display the camera, chessboard geometry after the first solve, and
exit. Used for debugging''')
parser.add_argument('--show-uncertainty-first-solve',
action = 'store_true',
help='''If given, display the uncertainty (at infinity) and observations after the
first solve, and exit. Used for debugging''')
parser.add_argument('--write-models-first-solve',
action = 'store_true',
help='''If given, write the solved models to disk after the first solve, and exit.
Used for debugging. Useful to check fov_x_deg when solving for a splined model''')
parser.add_argument('--which',
choices=('all-cameras-must-see-full-board',
'some-cameras-must-see-full-board',
'all-cameras-must-see-half-board',
'some-cameras-must-see-half-board'),
default='all-cameras-must-see-half-board',
help='''What kind of random poses are accepted. Default
is all-cameras-must-see-half-board''')
parser.add_argument('--fixed-frames',
action='store_true',
help='''Optimize the geometry keeping the chessboard frames fixed. This reduces the
freedom of the solution, and produces more confident
calibrations. It's possible to use this in reality by
surverying the chessboard poses''')
parser.add_argument('--method',
choices=('mean-pcam', 'bestq', 'cross-reprojection-rrp-Jfp'),
default='mean-pcam',
help='''Multiple uncertainty quantification methods are available. We default to 'mean-pcam' ''')
parser.add_argument('--ymax',
type=float,
default = 10.0,
help='''If given, use this as the upper extent of the uncertainty plot.''')
parser.add_argument('--uncertainty-at-range-sampled-min',
type=float,
help='''If given, use this as the lower bound of the ranges we look at when
evaluating projection confidence''')
parser.add_argument('--uncertainty-at-range-sampled-max',
type=float,
help='''If given, use this as the upper bound of the ranges we look at when
evaluating projection confidence''')
parser.add_argument('--explore',
action='store_true',
help='''Drop into a REPL at the end''')
parser.add_argument('--scan',
type=str,
default='',
choices=('range',
'tilt_radius',
'Nframes',
'Ncameras',
'object_width_n',
'object_spacing',
'num_far_constant_Nframes_near',
'num_far_constant_Nframes_all'),
help='''Study the effect of some parameter on uncertainty. The parameter is given as
an argument ot this function. Valid choices:
('range','tilt_radius','Nframes','Ncameras',
'object_width_n', 'object_spacing', 'num_far_constant_Nframes_near',
'num_far_constant_Nframes_all'). Scanning object-width-n
keeps the board size constant''')
parser.add_argument('--scan-object-spacing-compensate-range-from',
type=float,
help=f'''Only applies if --scan object-spacing. By
default we vary the object spacings without touching
anything else: the chessboard grows in size as we
increase the spacing. If given, we try to keep the
apparent size constant: as the object spacing grows, we
increase the range. The nominal range given in this
argument''')
parser.add_argument('--range',
default = '0.5,4.0',
type=str,
help='''if --scan num_far_...: this is "NEAR,FAR"; specifying the near and far ranges
to the chessboard to evaluate. if --scan range this is
"MIN,MAX"; specifying the extents of the ranges to
evaluate. Otherwise: this is RANGE, the one range of
chessboards to evaluate''')
parser.add_argument('--tilt-radius',
default='30.',
type=str,
help='''The radius of the uniform distribution used to
sample the pitch and yaw of the chessboard
observations, in degrees. The default is 30, meaning
that the chessboard pitch and yaw are sampled from
[-30deg,30deg]. if --scan tilt_radius: this is
TILT-MIN,TILT-MAX specifying the bounds of the two
tilt-radius values to evaluate. Otherwise: this is the
one value we use''')
parser.add_argument('--roll-radius',
type=float,
default=20.,
help='''The synthetic chessboard orientation is sampled
from a uniform distribution: [-RADIUS,RADIUS]. The
pitch,yaw radius is specified by the --tilt-radius. The
roll radius is selected here. "Roll" is the rotation
around the axis normal to the chessboard plane. Default
is 20deg''')
parser.add_argument('--x-radius',
type=float,
default=None,
help='''The synthetic chessboard position is sampled
from a uniform distribution: [-RADIUS,RADIUS]. The x
radius is selected here. "x" is direction in the
chessboard plane, and is also the axis along which the
cameras are distributed. A resonable default (possibly
range-dependent) is chosen if omitted. MUST be None if
--scan num_far_...''')
parser.add_argument('--y-radius',
type=float,
default=None,
help='''The synthetic chessboard position is sampled
from a uniform distribution: [-RADIUS,RADIUS]. The y
radius is selected here. "y" is direction in the
chessboard plane, and is also normal to the axis along
which the cameras are distributed. A resonable default
(possibly range-dependent) is chosen if omitted. MUST be
None if --scan num_far_...''')
parser.add_argument('--z-radius',
type=float,
default=None,
help='''The synthetic chessboard position is sampled
from a uniform distribution: [-RADIUS,RADIUS]. The z
radius is selected here. "z" is direction normal to the
chessboard plane. A resonable default (possibly
range-dependent) is chosen if omitted. MUST be None if
--scan num_far_...''')
parser.add_argument('--Ncameras',
default = '1',
type=str,
help='''How many cameras oberve our synthetic world. By default we just have one
camera. if --scan Ncameras: this is
NCAMERAS-MIN,NCAMERAS-MAX specifying the bounds of the
camera counts to evaluate. Otherwise: this is the one
Ncameras value to use''')
parser.add_argument('--Nframes',
type=str,
help='''How many observed frames we have. Ignored if --scan num_far_... if
--scan Nframes: this is NFRAMES-MIN,NFRAMES-MAX specifying the bounds of the
frame counts to evaluate. Otherwise: this is the one Nframes value to use''')
parser.add_argument('--Nframes-near',
type=positive_int,
help='''Used if --scan num_far_constant_Nframes_near. The number of "near" frames is
given by this argument, while we look at the effect of
adding more "far" frames.''')
parser.add_argument('--Nframes-all',
type=positive_int,
help='''Used if --scan num_far_constant_Nframes_all. The number of "near"+"far"
frames is given by this argument, while we look at the
effect of replacing "near" frames with "far" frames.''')
parser.add_argument('--Nscan-samples',
type=positive_int,
default=8,
help='''How many values of the parameter being scanned to evaluate. If we're scanning
something (--scan ... given) then by default the scan
evaluates 8 different values. Otherwise this is set to 1''')
parser.add_argument('--hardcopy',
help=f'''Filename to plot into. If omitted, we make an interactive plot. This is
passed directly to gnuplotlib''')
parser.add_argument('--terminal',
help=f'''gnuplot terminal to use for the plots. This is passed directly to
gnuplotlib. Omit this unless you know what you're doing''')
parser.add_argument('--extratitle',
help=f'''Extra title string to add to a plot''')
parser.add_argument('--title',
help=f'''Full title string to use in a plot. Overrides
the default and --extratitle''')
parser.add_argument('--set',
type=str,
action='append',
help='''Extra 'set' directives to gnuplotlib. Can be given multiple times''')
parser.add_argument('--unset',
type=str,
action='append',
help='''Extra 'unset' directives to gnuplotlib. Can be given multiple times''')
parser.add_argument('model',
type = str,
help='''Baseline camera model. I use the intrinsics from this model to generate
synthetic data. We probably want the "true" model to not
be too crazy, so this should probably by a parametric
(not splined) model''')
args = parser.parse_args()
if args.title is not None and args.extratitle is not None:
print("--title and --extratitle are mutually exclusive", file=sys.stderr)
sys.exit(1)
return args
args = parse_args()
if args.object_width_n is None and \
args.object_height_n is None:
args.object_width_n = "10"
args.object_height_n = 10
elif not ( args.object_width_n is not None and \
args.object_height_n is not None) and \
args.scan != 'object_width_n':
raise Exception("Either --object-width-n or --object-height-n are given: you must pass both or neither")
# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README
import numpy as np
import numpysane as nps
import gnuplotlib as gp
import copy
import os.path
# I import the LOCAL mrcal
scriptdir = os.path.dirname(os.path.realpath(__file__))
sys.path[:0] = f"{scriptdir}/../..",
import mrcal
def split_list(s, t):
r'''Splits a comma-separated list
None -> None
"A,B,C" -> (A,B,C)
"A" -> A
'''
if s is None: return None,0
l = [t(x) for x in s.split(',')]
if len(l) == 1: return l[0],1
return l,len(l)
def first(s):
r'''first in a list, or the value if a scalar'''
if hasattr(s, '__iter__'): return s[0]
return s
def last(s):
r'''last in a list, or the value if a scalar'''
if hasattr(s, '__iter__'): return s[-1]
return s
controllable_args = \
dict( tilt_radius = dict(_type = float),
range = dict(_type = float),
Ncameras = dict(_type = int),
Nframes = dict(_type = int),
object_spacing = dict(_type = float),
object_width_n = dict(_type = int) )
for a in controllable_args.keys():
l,n = split_list(getattr(args, a), controllable_args[a]['_type'])
controllable_args[a]['value'] = l
controllable_args[a]['listlen'] = n
if any( controllable_args[a]['listlen'] > 2 for a in controllable_args.keys() ):
raise Exception(f"All controllable args must have either at most 2 values. {a} has {controllable_args[a]['listlen']}")
controllable_arg_0values = [ a for a in controllable_args.keys() if controllable_args[a]['listlen'] == 0 ]
controllable_arg_2values = [ a for a in controllable_args.keys() if controllable_args[a]['listlen'] == 2 ]
if len(controllable_arg_2values) == 0: controllable_arg_2values = ''
elif len(controllable_arg_2values) == 1: controllable_arg_2values = controllable_arg_2values[0]
else: raise Exception(f"At most 1 controllable arg may have 2 values. Instead I saw: {controllable_arg_2values}")
if re.match("num_far_constant_Nframes_", args.scan):
if args.x_radius is not None or \
args.y_radius is not None or \
args.z_radius is not None:
raise Exception("--x-radius and --y-radius and --z-radius are exclusive with --scan num_far_...")
# special case
if 'Nframes' not in controllable_arg_0values:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes must not have been given")
if 'range' != controllable_arg_2values:
raise Exception(f"I'm scanning '{args.scan}', so --range must have 2 values")
if args.scan == "num_far_constant_Nframes_near":
if args.Nframes_all is not None:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes-all must not have have been given")
if args.Nframes_near is None:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes-near must have have been given")
else:
if args.Nframes_near is not None:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes-near must not have have been given")
if args.Nframes_all is None:
raise Exception(f"I'm scanning '{args.scan}', so --Nframes-all must have have been given")
else:
if args.scan != controllable_arg_2values:
# This covers the scanning-nothing-no2value-anything case
raise Exception(f"I'm scanning '{args.scan}', the arg given 2 values is '{controllable_arg_2values}'. They must match")
if len(controllable_arg_0values):
raise Exception(f"I'm scanning '{args.scan}', so all controllable args should have some value. Missing: '{controllable_arg_0values}")
if args.scan == '':
args.Nscan_samples = 1
Nuncertainty_at_range_samples = 80
uncertainty_at_range_sampled_min = args.uncertainty_at_range_sampled_min
if uncertainty_at_range_sampled_min is None:
uncertainty_at_range_sampled_min = first(controllable_args['range']['value'])/10.
uncertainty_at_range_sampled_max = args.uncertainty_at_range_sampled_max
if uncertainty_at_range_sampled_max is None:
uncertainty_at_range_sampled_max = last(controllable_args['range']['value']) *10.
uncertainty_at_range_samples = \
np.logspace( np.log10(uncertainty_at_range_sampled_min),
np.log10(uncertainty_at_range_sampled_max),
Nuncertainty_at_range_samples)
# I want the RNG to be deterministic
np.random.seed(0)
model_intrinsics = mrcal.cameramodel(args.model)
calobject_warp_true_ref = np.array(args.calobject_warp)
def solve(Ncameras,
Nframes_near, Nframes_far,
object_spacing,
models_true,
# q.shape = (Nframes, Ncameras, object_height, object_width, 2)
# Rt_ref_board.shape = (Nframes, 4,3)
q_true_near, Rt_ref_board_true_near,
q_true_far, Rt_ref_board_true_far,
fixed_frames = args.fixed_frames):
q_true_near = q_true_near [:Nframes_near]
Rt_ref_board_true_near = Rt_ref_board_true_near[:Nframes_near]
if q_true_far is not None:
q_true_far = q_true_far [:Nframes_far ]
Rt_ref_board_true_far = Rt_ref_board_true_far [:Nframes_far ]
else:
q_true_far = np.zeros( (0,) + q_true_near.shape[1:],
dtype = q_true_near.dtype)
Rt_ref_board_true_far = np.zeros( (0,) + Rt_ref_board_true_near.shape[1:],
dtype = Rt_ref_board_true_near.dtype)
calobject_warp_true = calobject_warp_true_ref.copy()
Nframes_all = Nframes_near + Nframes_far
Rt_ref_board_true = nps.glue( Rt_ref_board_true_near,
Rt_ref_board_true_far,
axis = -3 )
# Dense observations. All the cameras see all the boards
indices_frame_camera = np.zeros( (Nframes_all*Ncameras, 2), dtype=np.int32)
indices_frame = indices_frame_camera[:,0].reshape(Nframes_all,Ncameras)
indices_frame.setfield(nps.outer(np.arange(Nframes_all, dtype=np.int32),
np.ones((Ncameras,), dtype=np.int32)),
dtype = np.int32)
indices_camera = indices_frame_camera[:,1].reshape(Nframes_all,Ncameras)
indices_camera.setfield(nps.outer(np.ones((Nframes_all,), dtype=np.int32),
np.arange(Ncameras, dtype=np.int32)),
dtype = np.int32)
indices_frame_camintrinsics_camextrinsics = \
nps.glue(indices_frame_camera,
indices_frame_camera[:,(1,)],
axis=-1)
# If not fixed_frames: we use camera0 as the reference cordinate system, and
# we allow the chessboard poses to move around. Else: the reference
# coordinate system is arbitrary, but all cameras are allowed to move
# around. The chessboards poses are fixed
if not fixed_frames:
indices_frame_camintrinsics_camextrinsics[:,2] -= 1
q = nps.glue( q_true_near,
q_true_far,
axis = -5 )
# apply noise
q += np.random.randn(*q.shape) * args.observed_pixel_uncertainty
# The observations are dense (in the data every camera sees all the
# chessboards), but some of the observations WILL be out of bounds. I
# pre-mark those as outliers so that the solve doesn't do weird stuff
# Set the weights to 1 initially
# shape (Nframes, Ncameras, object_height_n, object_width_n, 3)
observations = nps.glue(q,
np.ones( q.shape[:-1] + (1,) ),
axis = -1)
# shape (Ncameras, 1, 1, 2)
imagersizes = nps.mv( nps.cat(*[ m.imagersize() for m in models_true ]),
-2, -4 )
# mark the out-of-view observations as outliers
observations[ np.any( q < 0, axis=-1 ), 2 ] = -1.
observations[ np.any( q-imagersizes >= 0, axis=-1 ), 2 ] = -1.
# shape (Nobservations, Nh, Nw, 2)
observations = nps.clump( observations,
n = 2 )
intrinsics = nps.cat( *[m.intrinsics()[1] for m in models_true] )
# If not fixed_frames: we use camera0 as the reference cordinate system, and
# we allow the chessboard poses to move around. Else: the reference
# coordinate system is arbitrary, but all cameras are allowed to move
# around. The chessboards poses are fixed
if fixed_frames:
extrinsics = nps.cat( *[m.rt_cam_ref() for m in models_true] )
else:
extrinsics = nps.cat( *[m.rt_cam_ref() for m in models_true[1:]] )
if len(extrinsics) == 0: extrinsics = None
if nps.norm2(models_true[0].rt_cam_ref()) > 1e-6:
raise Exception("models_true[0] must sit at the origin")
imagersizes = nps.cat( *[m.imagersize() for m in models_true] )
optimization_inputs = \
dict( # intrinsics filled in later
rt_cam_ref = extrinsics,
rt_ref_frame = mrcal.rt_from_Rt(Rt_ref_board_true),
points = None,
observations_board = observations,
indices_frame_camintrinsics_camextrinsics = indices_frame_camintrinsics_camextrinsics,
observations_point = None,
indices_point_camintrinsics_camextrinsics = None,
# lensmodel filled in later
calobject_warp = copy.deepcopy(calobject_warp_true),
imagersizes = imagersizes,
calibration_object_spacing = object_spacing,
verbose = False,
# do_optimize_extrinsics filled in later
# do_optimize_intrinsics_core filled in later
do_optimize_frames = False,
do_optimize_intrinsics_distortions = True,
do_optimize_calobject_warp = False, # turn this on, and reoptimize later, if needed
do_apply_regularization = True,
do_apply_outlier_rejection = False)
if args.lensmodel is None:
lensmodel = model_intrinsics.intrinsics()[0]
else:
lensmodel = args.lensmodel
Nintrinsics = mrcal.lensmodel_num_params(lensmodel)
if re.search("SPLINED", lensmodel):
# These are already mostly right, So I lock them down while I seed the
# intrinsics
optimization_inputs['do_optimize_extrinsics'] = False
# I pre-optimize the core, and then lock it down
optimization_inputs['lensmodel'] = 'LENSMODEL_STEREOGRAPHIC'
optimization_inputs['intrinsics'] = intrinsics[:,:4].copy()
optimization_inputs['do_optimize_intrinsics_core'] = True
stats = mrcal.optimize(**optimization_inputs)
print(f"## optimized. rms = {stats['rms_reproj_error__pixels']}", file=sys.stderr)
# core is good. Lock that down, and get an estimate for the control
# points
optimization_inputs['do_optimize_intrinsics_core'] = False
optimization_inputs['lensmodel'] = lensmodel
optimization_inputs['intrinsics'] = nps.glue(optimization_inputs['intrinsics'],
np.zeros((Ncameras,Nintrinsics-4),),axis=-1)
stats = mrcal.optimize(**optimization_inputs)
print(f"## optimized. rms = {stats['rms_reproj_error__pixels']}", file=sys.stderr)
# Ready for a final reoptimization with the geometry
optimization_inputs['do_optimize_extrinsics'] = True
if not fixed_frames:
optimization_inputs['do_optimize_frames'] = True
else:
optimization_inputs['lensmodel'] = lensmodel
if not mrcal.lensmodel_metadata_and_config(lensmodel)['has_core'] or \
not mrcal.lensmodel_metadata_and_config(model_intrinsics.intrinsics()[0])['has_core']:
raise Exception("I'm assuming all the models here have a core. It's just lazy coding. If you see this, feel free to fix.")
if lensmodel == model_intrinsics.intrinsics()[0]:
# Same model. Grab the intrinsics. They're 99% right
optimization_inputs['intrinsics'] = intrinsics.copy()
else:
# Different model. Grab the intrinsics core, optimize the rest
optimization_inputs['intrinsics'] = nps.glue(intrinsics[:,:4],
np.zeros((Ncameras,Nintrinsics-4),),axis=-1)
optimization_inputs['do_optimize_intrinsics_core'] = True
optimization_inputs['do_optimize_extrinsics'] = True
if not fixed_frames:
optimization_inputs['do_optimize_frames'] = True
stats = mrcal.optimize(**optimization_inputs)
print(f"## optimized. rms = {stats['rms_reproj_error__pixels']}", file=sys.stderr)
if not args.skip_calobject_warp_solve:
optimization_inputs['do_optimize_calobject_warp'] = True
stats = mrcal.optimize(**optimization_inputs)
print(f"## optimized. rms = {stats['rms_reproj_error__pixels']}", file=sys.stderr)
return optimization_inputs
def observation_centroid(optimization_inputs, icam):
r'''mean pixel coordinate of all non-outlier points seen by a given camera'''
ifcice = optimization_inputs['indices_frame_camintrinsics_camextrinsics']
observations = optimization_inputs['observations_board']
# pick the camera I want
observations = observations[ifcice[:,1] == icam]
# ignore outliers
q = observations[ (observations[...,2] > 0), :2]
return np.mean(q, axis=-2)
def eval_one_rangenear_tilt(models_true,
range_near, range_far, tilt_radius,
object_width_n, object_height_n, object_spacing,
uncertainty_at_range_samples,
Ncameras,
Nframes_near_samples, Nframes_far_samples):
# I want the RNG to be deterministic
np.random.seed(0)
uncertainties = np.zeros((len(Nframes_far_samples),
len(uncertainty_at_range_samples)),
dtype=float)
radius_cameras = (args.camera_spacing * (Ncameras-1)) / 2.
x_radius = args.x_radius if args.x_radius is not None else range_near*2. + radius_cameras
y_radius = args.y_radius if args.y_radius is not None else range_near*2.
z_radius = args.z_radius if args.z_radius is not None else range_near/10.
# shapes (Nframes, Ncameras, Nh, Nw, 2),
# (Nframes, 4,3)
q_true_near, Rt_ref_board_true_near = \
mrcal.synthesize_board_observations(models_true,
object_width_n = object_width_n,
object_height_n = object_height_n,
object_spacing = object_spacing,
calobject_warp = calobject_warp_true_ref,
rt_ref_boardcenter = np.array((0., 0., 0., radius_cameras, 0, range_near,)),
rt_ref_boardcenter__noiseradius = \
np.array((np.pi/180. * tilt_radius,
np.pi/180. * tilt_radius,
np.pi/180. * args.roll_radius,
x_radius, y_radius, z_radius)),
Nframes = np.max(Nframes_near_samples),
which = args.which)
if range_far is not None:
q_true_far, Rt_ref_board_true_far = \
mrcal.synthesize_board_observations(models_true,
object_width_n = object_width_n,
object_height_n = object_height_n,
object_spacing = object_spacing,
calobject_warp = calobject_warp_true_ref,
rt_ref_boardcenter = np.array((0., 0., 0., radius_cameras, 0, range_far,)),
rt_ref_boardcenter__noiseradius = \
np.array((np.pi/180. * tilt_radius,
np.pi/180. * tilt_radius,
np.pi/180. * args.roll_radius,
range_far*2. + radius_cameras,
range_far*2.,
range_far/10.)),
Nframes = np.max(Nframes_far_samples),
which = args.which)
else:
q_true_far = None
Rt_ref_board_true_far = None
for i_Nframes_far in range(len(Nframes_far_samples)):
Nframes_far = Nframes_far_samples [i_Nframes_far]
Nframes_near = Nframes_near_samples[i_Nframes_far]
optimization_inputs = solve(Ncameras,
Nframes_near, Nframes_far,
object_spacing,
models_true,
q_true_near, Rt_ref_board_true_near,
q_true_far, Rt_ref_board_true_far)
models_out = \
[ mrcal.cameramodel( optimization_inputs = optimization_inputs,
icam_intrinsics = icam ) \
for icam in range(Ncameras) ]
model = models_out[args.icam_uncertainty]
if args.show_geometry_first_solve:
mrcal.show_geometry(models_out,
show_calobjects = True,
wait = True)
sys.exit()
if args.show_uncertainty_first_solve:
mrcal.show_projection_uncertainty(model,
method = args.method,
observations= True,
wait = True)
sys.exit()
if args.write_models_first_solve:
for i in range(len(models_out)):
f = f"/tmp/camera{i}.cameramodel"
if os.path.exists(f):
input(f"File {f} already exists, and I want to overwrite it. Press enter to overwrite. Ctrl-c to exit")
models_out[i].write(f)
print(f"Wrote {f}")
sys.exit()
# shape (N,3)
# I sample the center of the imager
pcam_samples = \
mrcal.unproject( observation_centroid(optimization_inputs,
args.icam_uncertainty),
*model.intrinsics(),
normalize = True) * \
nps.dummy(uncertainty_at_range_samples, -1)
uncertainties[i_Nframes_far] = \
mrcal.projection_uncertainty(pcam_samples,
model,
method = args.method,
what = 'worstdirection-stdev')
return uncertainties
output_table_legend = 'range_uncertainty_sample Nframes_near Nframes_far Ncameras range_near range_far tilt_radius object_width_n object_spacing uncertainty'
output_table_fmt = '%f %d %d %d %f %f %f %d %f %f'
output_table_icol__range_uncertainty_sample = 0
output_table_icol__Nframes_near = 1
output_table_icol__Nframes_far = 2
output_table_icol__Ncameras = 3
output_table_icol__range_near = 4
output_table_icol__range_far = 5
output_table_icol__tilt_radius = 6
output_table_icol__object_width_n = 7
output_table_icol__object_spacing = 8
output_table_icol__uncertainty = 9
output_table_Ncols = 10
output_table = np.zeros( (args.Nscan_samples, Nuncertainty_at_range_samples, output_table_Ncols), dtype=float)
output_table[:,:, output_table_icol__range_uncertainty_sample] += uncertainty_at_range_samples
if re.match("num_far_constant_Nframes_", args.scan):
Nfar_samples = args.Nscan_samples
if args.scan == "num_far_constant_Nframes_near":
Nframes_far_samples = np.linspace(0,
args.Nframes_near//4,
Nfar_samples, dtype=int)
Nframes_near_samples = Nframes_far_samples*0 + args.Nframes_near
else:
Nframes_far_samples = np.linspace(0,
args.Nframes_all,
Nfar_samples, dtype=int)
Nframes_near_samples = args.Nframes_all - Nframes_far_samples
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
rt_ref_cam = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
# shape (args.Nscan_samples, Nuncertainty_at_range_samples)
output_table[:,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
*controllable_args['range']['value'],
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)
output_table[:,:, output_table_icol__Nframes_near] += nps.transpose(Nframes_near_samples)
output_table[:,:, output_table_icol__Nframes_far] += nps.transpose(Nframes_far_samples)
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value'][0]
output_table[:,:, output_table_icol__range_far] = controllable_args['range']['value'][1]
output_table[:,:, output_table_icol__tilt_radius ] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = Nframes_far_samples
elif args.scan == "range":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
rt_ref_cam = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
Nrange_samples = args.Nscan_samples
range_samples = np.linspace(*controllable_args['range']['value'],
Nrange_samples, dtype=float)
for i_range in range(Nrange_samples):
output_table[i_range,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
range_samples[i_range], None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] += nps.transpose(range_samples)
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius ] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = range_samples
elif args.scan == "tilt_radius":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
rt_ref_cam = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
Ntilt_rad_samples = args.Nscan_samples
tilt_rad_samples = np.linspace(*controllable_args['tilt_radius']['value'],
Ntilt_rad_samples, dtype=float)
for i_tilt in range(Ntilt_rad_samples):
output_table[i_tilt,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
tilt_rad_samples[i_tilt],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] += nps.transpose(tilt_rad_samples)
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = tilt_rad_samples
elif args.scan == "Ncameras":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
N_Ncameras_samples = args.Nscan_samples
Ncameras_samples = np.linspace(*controllable_args['Ncameras']['value'],
N_Ncameras_samples, dtype=int)
for i_Ncameras in range(N_Ncameras_samples):
Ncameras = Ncameras_samples[i_Ncameras]
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
rt_ref_cam = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(Ncameras) ]
output_table[i_Ncameras,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
Ncameras,
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] += nps.transpose(Ncameras_samples)
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = Ncameras_samples
elif args.scan == "Nframes":
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
rt_ref_cam = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
N_Nframes_samples = args.Nscan_samples
Nframes_samples = np.linspace(*controllable_args['Nframes']['value'],
N_Nframes_samples, dtype=int)
for i_Nframes in range(N_Nframes_samples):
Nframes = Nframes_samples[i_Nframes]
Nframes_near_samples = np.array( (Nframes,), dtype=int)
output_table[i_Nframes,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near]+= nps.transpose(Nframes_samples)
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = Nframes_samples
elif args.scan == "object_width_n":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
rt_ref_cam = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
Nsamples = args.Nscan_samples
samples = np.linspace(*controllable_args['object_width_n']['value'],
Nsamples, dtype=int)
# As I move the width, I adjust the spacing to keep the total board size
# constant. The object spacing in the argument applies to the MIN value of
# the object_width_n.
W = (controllable_args['object_width_n']['value'][0]-1) * controllable_args['object_spacing']['value']
object_spacing = W / samples
for i_sample in range(Nsamples):
output_table[i_sample,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
controllable_args['tilt_radius']['value'],
samples[i_sample],
samples[i_sample],
object_spacing[i_sample],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ]+= nps.transpose(samples)
output_table[:,:, output_table_icol__object_spacing ]+= nps.transpose(object_spacing)
elif args.scan == "object_spacing":
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
rt_ref_cam = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
Nsamples = args.Nscan_samples
samples = np.linspace(*controllable_args['object_spacing']['value'],
Nsamples, dtype=float)
# As I move the spacing, I leave object_width_n, letting the total board
# size change. The object spacing in the argument applies to the MIN value
# of the object_width_n.
for i_sample in range(Nsamples):
r = controllable_args['range']['value']
if args.scan_object_spacing_compensate_range_from:
r *= samples[i_sample]/args.scan_object_spacing_compensate_range_from
output_table[i_sample,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
r, None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
samples[i_sample],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ]+= nps.transpose(samples)
if args.scan_object_spacing_compensate_range_from:
output_table[:,:, output_table_icol__range_near] += controllable_args['range']['value'] * nps.transpose(samples/samples[0])
else:
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
else:
# no --scan. We just want one sample
Nframes_near_samples = np.array( (controllable_args['Nframes']['value'],), dtype=int)
Nframes_far_samples = np.array( (0,), dtype=int)
models_true = \
[ mrcal.cameramodel(intrinsics = model_intrinsics.intrinsics(),
imagersize = model_intrinsics.imagersize(),
rt_ref_cam = np.array((0,0,0,
i*args.camera_spacing,
0,0), dtype=float) ) \
for i in range(controllable_args['Ncameras']['value']) ]
output_table[0,:, output_table_icol__uncertainty] = \
eval_one_rangenear_tilt(models_true,
controllable_args['range']['value'], None,
controllable_args['tilt_radius']['value'],
controllable_args['object_width_n']['value'],
args.object_height_n,
controllable_args['object_spacing']['value'],
uncertainty_at_range_samples,
controllable_args['Ncameras']['value'],
Nframes_near_samples, Nframes_far_samples)[0]
output_table[:,:, output_table_icol__Nframes_near] = controllable_args['Nframes']['value']
output_table[:,:, output_table_icol__Nframes_far] = 0
output_table[:,:, output_table_icol__Ncameras] = controllable_args['Ncameras']['value']
output_table[:,:, output_table_icol__range_near] = controllable_args['range']['value']
output_table[:,:, output_table_icol__range_far] = -1
output_table[:,:, output_table_icol__tilt_radius] = controllable_args['tilt_radius']['value']
output_table[:,:, output_table_icol__object_width_n ] = controllable_args['object_width_n']['value']
output_table[:,:, output_table_icol__object_spacing ] = controllable_args['object_spacing']['value']
samples = None
if isinstance(controllable_args['range']['value'], float):
guides = [ f"arrow nohead dashtype 3 from {controllable_args['range']['value']},graph 0 to {controllable_args['range']['value']},graph 1" ]
else:
guides = [ f"arrow nohead dashtype 3 from {r},graph 0 to {r},graph 1" for r in controllable_args['range']['value'] ]
guides.append(f"arrow nohead dashtype 3 from graph 0,first {args.observed_pixel_uncertainty} to graph 1,first {args.observed_pixel_uncertainty}")
title = args.title
if args.scan == "num_far_constant_Nframes_near":
if title is None:
title = f"Scanning 'far' observations added to a set of 'near' observations. Have {controllable_args['Ncameras']['value']} cameras, {args.Nframes_near} 'near' observations, at ranges {controllable_args['range']['value']}."
legend_what = 'Nframes_far'
elif args.scan == "num_far_constant_Nframes_all":
if title is None:
title = f"Scanning 'far' observations replacing 'near' observations. Have {controllable_args['Ncameras']['value']} cameras, {args.Nframes_all} total observations, at ranges {controllable_args['range']['value']}."
legend_what = 'Nframes_far'
elif args.scan == "Nframes":
if title is None:
title = f"Scanning Nframes. Have {controllable_args['Ncameras']['value']} cameras looking out at {controllable_args['range']['value']:.2f}m."
legend_what = 'Nframes'
elif args.scan == "Ncameras":
if title is None:
title = f"Scanning Ncameras. Observing {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m."
legend_what = 'Ncameras'
elif args.scan == "range":
if title is None:
title = f"Scanning the distance to observations. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards."
legend_what = 'Range-to-chessboards'
elif args.scan == "tilt_radius":
if title is None:
title = f"Scanning the board tilt. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m"
legend_what = 'Random chessboard tilt radius'
elif args.scan == "object_width_n":
if title is None:
title = f"Scanning the calibration object density, keeping the board size constant. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m"
legend_what = 'Number of chessboard points per side'
elif args.scan == "object_spacing":
if title is None:
if args.scan_object_spacing_compensate_range_from:
title = f"Scanning the calibration object spacing, keeping the point count constant, and letting the board grow. Range grows with spacing. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m"
else:
title = f"Scanning the calibration object spacing, keeping the point count constant, and letting the board grow. Range is constant. Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m"
legend_what = 'Distance between adjacent chessboard corners'
else:
# no --scan. We just want one sample
if title is None:
title = f"Have {controllable_args['Ncameras']['value']} cameras looking at {controllable_args['Nframes']['value']} boards at {controllable_args['range']['value']:.2f}m with tilt radius {controllable_args['tilt_radius']['value']}"
if args.extratitle is not None:
title = f"{title}: {args.extratitle}"
if samples is None:
legend = None
elif samples.dtype.kind == 'i':
legend = np.array([ f"{legend_what} = {x}" for x in samples])
else:
legend = np.array([ f"{legend_what} = {x:.2f}" for x in samples])
np.savetxt(sys.stdout,
nps.clump(output_table, n=2),
fmt = output_table_fmt,
header= output_table_legend)
plotoptions = \
dict( yrange = (0, args.ymax),
_with = 'lines',
_set = guides,
unset = 'grid',
title = title,
xlabel = 'Range (m)',
ylabel = 'Expected worst-direction uncertainty (pixels)',
hardcopy = args.hardcopy,
terminal = args.terminal,
wait = not args.explore and args.hardcopy is None)
if legend is not None: plotoptions['legend'] = legend
if args.set:
gp.add_plot_option(plotoptions,
_set = args.set)
if args.unset:
gp.add_plot_option(plotoptions,
_unset = args.unset)
gp.plot(uncertainty_at_range_samples,
output_table[:,:, output_table_icol__uncertainty],
**plotoptions)
if args.explore:
import IPython
IPython.embed()
sys.exit()
mrcal-2.5/analyses/extrinsics-stability.py 0000775 0000000 0000000 00000012536 15123677724 0021074 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Look at the extrinsics drift of a camera pair over time
Described here:
https://mrcal.secretsauce.net/docs-2.5/differencing.html#extrinsics-diff
'''
import sys
import numpy as np
import numpysane as nps
import gnuplotlib as gp
import mrcal
def compute_Rt_implied_01(*models):
lensmodels = [model.intrinsics()[0] for model in models]
intrinsics_data = [model.intrinsics()[1] for model in models]
# v shape (...,Ncameras,Nheight,Nwidth,...)
# q0 shape (..., Nheight,Nwidth,...)
v,q0 = \
mrcal.sample_imager_unproject(60, None,
*models[0].imagersize(),
lensmodels, intrinsics_data,
normalize = True)
distance = (1.0, 100.)
focus_center = None
focus_radius = 500
if distance is None:
atinfinity = True
distance = np.ones((1,), dtype=float)
else:
atinfinity = False
distance = nps.atleast_dims(np.array(distance), -1)
distance = nps.mv(distance.ravel(), -1,-4)
if focus_center is None:
focus_center = (models[0].imagersize() - 1.)/2.
implied_Rt10 = \
mrcal.implied_Rt10__from_unprojections(q0,
# shape (len(distance),Nheight,Nwidth,3)
v[0,...] * distance,
v[1,...],
atinfinity = atinfinity,
focus_center = focus_center,
focus_radius = focus_radius)
return mrcal.invert_Rt(implied_Rt10)
models_filenames = sys.argv[1:5]
models = [mrcal.cameramodel(f) for f in models_filenames]
pairs = ( ( models[0],models[1]),
( models[2],models[3]) )
# The "before" extrinsic transformation
m0,m1 = pairs[0]
Rt01 = mrcal.compose_Rt( m0.Rt_cam_ref(),
m1.Rt_ref_cam())
# The "after" extrinsics transformation. I remap both cameras into the "before"
# space, so that we're looking at the extrinsics transformation in the "before"
# coord system. This will allow us to compare before and after
#
# Rt_implied__0before_0after Rt_0after_1after Rt_implied__1after_1before
Rt_implied__0before_0after = compute_Rt_implied_01(pairs[0][0], pairs[1][0])
Rt_implied__1after_1before = compute_Rt_implied_01(pairs[1][1], pairs[0][1])
m0,m1 = pairs[1]
Rt_0after_1after = \
mrcal.compose_Rt( m0.Rt_cam_ref(),
m1.Rt_ref_cam())
Rt01_after_extrinsicsbefore = \
mrcal.compose_Rt( Rt_implied__0before_0after,
Rt_0after_1after,
Rt_implied__1after_1before )
# I have the two relative transforms. If camera0 is fixed, how much am I moving
# camera1?
Rt_1before_1after = mrcal.compose_Rt(mrcal.invert_Rt(Rt01), Rt01_after_extrinsicsbefore)
r_1before_1after = mrcal.r_from_R(Rt_1before_1after[:3,:])
t_1before_1after = Rt_1before_1after[3,:]
magnitude = nps.mag(t_1before_1after)
direction = t_1before_1after/magnitude
angle = nps.mag(r_1before_1after)
axis = r_1before_1after/angle
angle_deg = angle*180./np.pi
np.set_printoptions(precision=2)
print(f"translation: {magnitude*1000:.2f}mm in the direction {direction}")
print(f"rotation: {angle_deg:.3f}deg around the axis {axis}")
for i in range(len(models)):
m = models[i]
qcenter,dq_dv,_ = mrcal.project(np.array((0,0,1.)),
*m.intrinsics(),
get_gradients=True)
# I now do a simple thing. I have v=[0,0,1] so dq_dv[:,2]=0. A small pitch
# gives me dv = (0,sinth,costh) ~ (0,th,1). So dq = dq_dv[:,1]*th +
# dq_dv[:,2] = dq_dv[:,1]*th so for a pitch: mag(dq/dth) = mag(dq_dv[:,1]).
# Similarly for a yaw I have mag(dq_dv[:,0]). I find the worst one, and call
# it good. I can do that because dq_dv will be diagonally dominant, and the
# diagonal elements will be very similar. mrcal.rectified_resolution() does
# this
resolution__pix_per_rad = np.max(nps.transpose(nps.mag(dq_dv[:,:2])))
resolution__pix_per_deg = resolution__pix_per_rad * np.pi/180.
if 0:
# More complicated, but probably not better. And not completed
#
# As the camera rotates, v shifts: rotate(v,r) ~ v + dv/dr dr, so the
# projection shifts to q + dq/dv dv = q + dq/dv dv/dr dr
#
# Rodrigues rotation formula. th = mag(r), axis = normalize(r) = r/th
#
# rotate(r,v) = v cos(th) + cross(axis, v) sin(th) + axis axist v (1 - cos(th))
#
# If th is small:
#
# rotate(r,v) = v + cross(axis, v) th
# = v + [ axis1*v2-axis2*v1 axis2*v0-axis0*v2 axis0*v1-axis1*v0] th
#
# v = [0,0,1] so
#
# rotate(r,v) = v + [ axis1 axis0 0] th
# = v + [ r1 r0 0 ]
#
# So
dv_dr = np.array(((0,1,0),
(1,0,0),
(0,0,0)))
dq_dr = nps.matmult(dq_dv,dv_dr)
# I have dq_dr2 = 0, so lets ignore it
dq_dr01 = dq_dr[:,:2]
# Can finish this now
print(f"Camera {i} has a resolution of {1./resolution__pix_per_deg:.3f} degrees per pixel at the center")
mrcal-2.5/analyses/mrcal-convert-lensmodel-from-kalibr-fov 0000775 0000000 0000000 00000044763 15123677724 0024006 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Convert a kalibr 1-parameter distortion model to something more standard
SYNOPSIS
$ analyses/mrcal-convert-lensmodel-from-kalibr-fov \
--num-trials 10 \
--radius 0 \
--viz LENSMODEL_OPENCV8 \
640 480 251.1 249.4 325.4 238.1 0.91
RMS error of this solution: 0.038970851989073026 pixels.
RMS error of this solution: 0.312567345983669 pixels.
RMS error of this solution: 0.03897085198907192 pixels.
RMS error of this solution: 0.31102107617891206 pixels.
RMS error of this solution: 0.03897085198907148 pixels.
RMS error of this solution: 0.31024552395361005 pixels.
RMS error of this solution: 0.038970851989072686 pixels.
RMS error of this solution: 0.03897085198907228 pixels.
RMS error of this solution: 0.31256734598368013 pixels.
RMS error of this solution: 232.49658418723286 pixels.
RMS error of the BEST solution: 0.03897085198907148 pixels.
# generated on 2025-11-10 23:53:53 with ./mrcal-convert-lensmodel-from-kalibr-fov --num-trials 10 --radius 0 --viz LENSMODEL_OPENCV8 640 480 251.1 249.4 325.4 238.1 0.91
{
'lensmodel': 'LENSMODEL_OPENCV8',
# intrinsics are fx,fy,cx,cy,distortion0,distortion1,....
'intrinsics': [ 269.7069092, 267.8824348, 325.3977834, 238.1017166, 0.4354014227, 0.02216823609, -1.263764376e-06, 1.317529587e-06, 6.778953407e-05, 0.7476900385, 0.09048943655, 0.001241101467,],
'rt_cam_ref': [ 0, 0, 0, 0, 0, 0,],
'extrinsics': [ 0, 0, 0, 0, 0, 0,], # for compatibility with mrcal < 2.5
'imagersize': [ 640, 480,],
}
This is a cut down 'mrcal-convert-lensmodel --sampled' for the kalibr "fov"
model specifically. This model isn't supported by mrcal, so that tool couldn't
be used directly. This is a quick hack to interoperate with tools that use this
model.
The kalibr models are documented here:
https://github.com/ethz-asl/kalibr/wiki/supported-models
See the docs for mrcal-convert-lensmodel for usage details
'''
import sys
import argparse
import re
import os
def parse_args():
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--gridn',
type=int,
default = (30,20),
nargs = 2,
help='''How densely we should sample the imager. By default we use
a 30x20 grid''')
parser.add_argument('--where',
type=float,
nargs=2,
help='''Used with or without --sampled. I use a subset
of the imager to compute the fit. The active region is a
circle centered on this point. If omitted, we will focus
on the center of the imager''')
parser.add_argument('--radius',
type=float,
help='''Used with or without --sampled. I use a subset
of the imager to compute the fit. The active region is a
circle with a radius given by this parameter. If radius
== 0, I'll use the whole imager for the fit. If radius <
0, this parameter specifies the width of the region at
the corners that I should ignore: I will use
sqrt(width^2 + height^2)/2. - abs(radius). This is valid
ONLY if we're focusing at the center of the imager. By
default I ignore a large-ish chunk area at the corners.''')
parser.add_argument('--viz',
action='store_true',
help='''Visualize the differences between the input and
output models''')
parser.add_argument('--cbmax',
type=float,
default=4,
help='''Maximum range of the colorbar''')
parser.add_argument('--title',
type=str,
default = None,
help='''Used if --viz. Title string for the diff plot.
Overrides the default title. Exclusive with
--extratitle''')
parser.add_argument('--extratitle',
type=str,
default = None,
help='''Used if --viz. Additional string for the plot to
append to the default title. Exclusive with --title''')
parser.add_argument('--hardcopy',
type=str,
help='''Used if --viz. Write the diff output to disk,
instead of making an interactive plot''')
parser.add_argument('--terminal',
type=str,
help=r'''Used if --viz. gnuplotlib terminal. The default
is good almost always, so most people don't need this
option''')
parser.add_argument('--set',
type=str,
action='append',
help='''Used if --viz. Extra 'set' directives to
gnuplotlib. Can be given multiple times''')
parser.add_argument('--unset',
type=str,
action='append',
help='''Used if --viz. Extra 'unset' directives to
gnuplotlib. Can be given multiple times''')
parser.add_argument('--force', '-f',
action='store_true',
default=False,
help='''By default existing models on disk are not
overwritten. Pass --force to overwrite them without
complaint''')
parser.add_argument('--outdir',
type=lambda d: d if os.path.isdir(d) else \
parser.error(f"--outdir requires an existing directory as the arg, but got '{d}'"),
help='''Directory to write the output into. If omitted,
we use the directory of the input model''')
parser.add_argument('--num-trials',
type = int,
default = 1,
help='''If given, run the solve more than once. Useful
in case random initialization produces noticeably
different results. By default we run just one trial''')
parser.add_argument('to',
type=str,
help='The target lens model')
parser.add_argument('w_h_fx_fy_cx_cy_s',
type=float,
nargs=7,
help='''Input camera model. This is "kalibr fov"
parameters. mrcal doesn't support this, so I use these
manually''')
args = parser.parse_args()
if args.title is not None and \
args.extratitle is not None:
print("Error: --title and --extratitle are exclusive", file=sys.stderr)
sys.exit(1)
return args
args = parse_args()
# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README
# I import the LOCAL mrcal
sys.path[:0] = f"{os.path.dirname(os.path.realpath(__file__))}/..",
import numpy as np
import numpysane as nps
import time
import copy
import mrcal
def fov_unproject_normalized( q, fxy, cxy, s ):
# The reference implementation is here:
# https://github.com/ethz-asl/kalibr/blob/1f60227442d25e36365ef5f72cd80b9666d73467/aslam_cv/aslam_cameras/include/aslam/cameras/implementation/FovDistortion.hpp#L89
# kMaxValidAngle is defined here:
# https://github.com/ethz-asl/kalibr/blob/1f60227442d25e36365ef5f72cd80b9666d73467/aslam_cv/aslam_cameras/include/aslam/cameras/FovDistortion.hpp#L146
# q has shape (..., 2)
# shape (..., 2)
xy = (q - cxy)/fxy
# shape (...)
r_d = nps.mag(xy)
i_invalid = r_d < 1e-6
r_d[i_invalid] = 1. # to not /0. Will do the "right" thing in a bit
mul2tanwby2 = 2.0 * np.tan(s / 2.0)
# shape (...)
r_u = np.tan(r_d * s) / mul2tanwby2 / r_d
# if r_d is ~0 then
# np.tan(r_d * s) / mul2tanwby2 / r_d ~
# np.tan(r_d * s) / r_d / mul2tanwby2 ~
# np.tan(r_d * s) / (r_d*s) * s / mul2tanwby2 ~
# s / mul2tanwby2 ~
r_u[i_invalid] = s / mul2tanwby2
# shape (...,2)
xy = \
nps.dummy(r_u, axis = -1) * \
xy
v = nps.glue(xy, np.ones(xy.shape[:-1] + (1,)),
axis = -1)
# normalize
v /= nps.dummy(nps.mag(v), axis=-1)
return v
lensmodel_to = args.to
try:
meta = mrcal.lensmodel_metadata_and_config(lensmodel_to)
except Exception as e:
print(f"Invalid lens model '{lensmodel_to}': couldn't get the metadata: {e}",
file=sys.stderr)
sys.exit(1)
if not meta['has_gradients']:
print(f"lens model {lensmodel_to} is not supported at this time: its gradients aren't implemented",
file=sys.stderr)
sys.exit(1)
try:
Ndistortions = mrcal.lensmodel_num_params(lensmodel_to) - 4
except:
print(f"Unknown lens model: '{lensmodel_to}'", file=sys.stderr)
sys.exit(1)
file_output = sys.stdout
dims = np.array(args.w_h_fx_fy_cx_cy_s[:2],
dtype = np.int32)
if args.radius is None:
# By default use 1/4 of the smallest dimension
args.radius = -np.min(dims) // 4
print(f"Default radius: {args.radius}. We're ignoring the regions {-args.radius} pixels from each corner",
file=sys.stderr)
if args.where is not None and \
nps.norm2(args.where - (dims - 1.) / 2) > 1e-3:
print("A radius <0 is only implemented if we're focusing on the imager center: use an explicit --radius, or omit --where",
file=sys.stderr)
sys.exit(1)
# Alrighty. Let's actually do the work. I do this:
#
# 1. Sample the imager space with the known model
# 2. Unproject to get the 3d observation vectors
# 3. Solve a new model that fits those vectors to the known observations, but
# using the new model
### I sample the pixels in an NxN grid
Nx,Ny = args.gridn
qx = np.linspace(0, dims[0]-1, Nx)
qy = np.linspace(0, dims[1]-1, Ny)
# q is (Ny*Nx, 2). Each slice of q[:] is an (x,y) pixel coord
q = np.ascontiguousarray( nps.transpose(nps.clump( nps.cat(*np.meshgrid(qx,qy)), n=-2)) )
if args.radius != 0:
# we use a subset of the input data for the fit
if args.where is None:
focus_center = (dims - 1.) / 2.
else:
focus_center = args.where
if args.radius > 0:
r = args.radius
else:
if nps.norm2(focus_center - (dims - 1.) / 2) > 1e-3:
print("A radius <0 is only implemented if we're focusing on the imager center",
file=sys.stderr)
sys.exit(1)
r = nps.mag(dims)/2. + args.radius
grid_off_center = q - focus_center
i = nps.norm2(grid_off_center) < r*r
q = q[i, ...]
# To visualize the sample grid:
# import gnuplotlib as gp
# gp.plot(q[:,0], q[:,1], _with='points pt 7 ps 2', xrange=[0,3904],yrange=[3904,0], wait=1, square=1)
# sys.exit()
### I unproject this, with broadcasting
# shape (Ny*Nx, 3)
p = fov_unproject_normalized( q,
np.array(args.w_h_fx_fy_cx_cy_s[2:4]),
np.array(args.w_h_fx_fy_cx_cy_s[4:6]),
args.w_h_fx_fy_cx_cy_s[6] )
# Ignore any failed unprojections
i_finite = np.isfinite(p[:,0])
p = p[i_finite]
q = q[i_finite]
Npoints = len(q)
weights = np.ones((Npoints,), dtype=float)
### Solve!
### I solve the optimization a number of times with different random seed
### values, taking the best-fitting results. This is required for the richer
### models such as LENSMODEL_OPENCV8
err_rms_best = 1e10
intrinsics_data_best = None
rt_cam_ref_best = None
for i in range(args.num_trials):
# random seed for the new intrinsics
intrinsics_core = np.array(args.w_h_fx_fy_cx_cy_s[2:6])
distortions = (np.random.rand(Ndistortions) - 0.5) * 1e-3 # random initial seed
intrinsics_to_values = nps.dummy(nps.glue(intrinsics_core, distortions, axis=-1),
axis=-2)
# each point has weight 1.0
observations_points = nps.glue(q, nps.transpose(weights), axis=-1)
observations_points = np.ascontiguousarray(observations_points) # must be contiguous. mrcal.optimize() should really be more lax here
# Which points we're observing. This is dense and kinda silly for this
# application. Each slice is (i_point,i_camera,i_camera-1). Initially O
# do everything in camera-0 coordinates, and I do not move the
# extrinsics
indices_point_camintrinsics_camextrinsics = np.zeros((Npoints,3), dtype=np.int32)
indices_point_camintrinsics_camextrinsics[:,0] = \
np.arange(Npoints, dtype=np.int32)
indices_point_camintrinsics_camextrinsics[:,1] = 0
indices_point_camintrinsics_camextrinsics[:,2] = -1
optimization_inputs = \
dict(intrinsics = intrinsics_to_values,
rt_cam_ref = None,
rt_ref_frame = None, # no frames. Just points
points = p,
observations_board = None, # no board observations
indices_frame_camintrinsics_camextrinsics = None, # no board observations
observations_point = observations_points,
indices_point_camintrinsics_camextrinsics = indices_point_camintrinsics_camextrinsics,
lensmodel = lensmodel_to,
imagersizes = nps.atleast_dims(dims, -2),
# I'm not optimizing the point positions (frames), so these
# need to be set to be inactive, and to include the ranges I do
# have
point_min_range = 1e-3,
point_max_range = 1e3,
# I optimize the lens parameters. That's the whole point
do_optimize_intrinsics_core = True,
do_optimize_intrinsics_distortions = True,
do_optimize_extrinsics = False,
# NOT optimizing the observed point positions
do_optimize_frames = False )
if re.match("LENSMODEL_SPLINED_STEREOGRAPHIC_", lensmodel_to):
# splined models have a core, but those variables are largely redundant
# with the spline parameters. So I lock down the core when targetting
# splined models
optimization_inputs['do_optimize_intrinsics_core'] = False
stats = mrcal.optimize(**optimization_inputs,
# No outliers. I have the points that I have
do_apply_outlier_rejection = False,
verbose = False)
err_rms = stats['rms_reproj_error__pixels']
print(f"RMS error of this solution: {err_rms} pixels.",
file=sys.stderr)
if err_rms < err_rms_best:
err_rms_best = err_rms
intrinsics_data_best = optimization_inputs['intrinsics'][0,:].copy()
if intrinsics_data_best is None:
print("No valid intrinsics found!", file=sys.stderr)
sys.exit(1)
if args.num_trials > 1:
print(f"RMS error of the BEST solution: {err_rms_best} pixels.",
file=sys.stderr)
m_to = mrcal.cameramodel( intrinsics = (lensmodel_to, intrinsics_data_best.ravel()),
imagersize = dims )
note = \
"generated on {} with {}\n". \
format(time.strftime("%Y-%m-%d %H:%M:%S"),
' '.join(mrcal.shellquote(s) for s in sys.argv))
m_to.write(file_output, note=note)
if isinstance(file_output, str):
print(f"Wrote '{file_output}'",
file=sys.stderr)
if args.viz:
plotkwargs_extra = {}
if args.set is not None:
plotkwargs_extra['set'] = args.set
if args.unset is not None:
plotkwargs_extra['unset'] = args.unset
if args.title is not None:
plotkwargs_extra['title'] = args.title
# I compute the reprojections again. Similar to the code used in the solve,
# but the spacing might be different AND I do not ignore corners
gridn_width,gridn_height = 80,50
qx = np.linspace(0, dims[0]-1, gridn_width)
qy = np.linspace(0, dims[1]-1, gridn_height)
# shape (Ny,Nx,2)
q = np.ascontiguousarray( nps.mv( nps.cat(*np.meshgrid(qx,qy)),
0,-1))
p = fov_unproject_normalized( q,
np.array(args.w_h_fx_fy_cx_cy_s[2:4]),
np.array(args.w_h_fx_fy_cx_cy_s[4:6]),
args.w_h_fx_fy_cx_cy_s[6] )
q_to = mrcal.project(p, *m_to.intrinsics())
diff = q - q_to
difflen = nps.mag(diff)
import gnuplotlib as gp
plotkwargs_extra['hardcopy'] = args.hardcopy
plotkwargs_extra['terminal'] = args.terminal
contour_increment = None
contour_labels_styles = 'boxed'
contour_labels_font = None
if 'title' not in plotkwargs_extra:
title = f"Diff in fitted fov model to {lensmodel_to}"
if args.extratitle is not None:
title += ": " + args.extratitle
plotkwargs_extra['title'] = title
plot_options = plotkwargs_extra
gp.add_plot_option(plot_options,
cbrange = [0,args.cbmax])
color = difflen
# Any invalid values (nan or inf) are set to an effectively infinite
# difference
color[~np.isfinite(color)] = 1e6
curve_options = \
mrcal.visualization._options_heatmap_with_contours(
# update these plot options
plotkwargs_extra,
contour_max = args.cbmax,
contour_increment = contour_increment,
imagersize = args.w_h_fx_fy_cx_cy_s[:2],
gridn_width = gridn_width,
gridn_height = gridn_height,
contour_labels_styles = contour_labels_styles,
contour_labels_font = contour_labels_font,
do_contours = True)
plot_data_args = [ (color, curve_options) ]
data_tuples = plot_data_args
plot = gp.gnuplotlib(**plot_options)
plot.plot(*data_tuples)
if args.hardcopy is None:
plot.wait()
mrcal-2.5/analyses/mrcal-pick-features 0000775 0000000 0000000 00000155165 15123677724 0020114 0 ustar 00root root 0000000 0000000 #!/usr/bin/python3
r'''Facilitate interactive human-in-the-loop corresponding-feature picking
And use features to solve for extrinsics in a stereo pair. This needs a clearer,
more general purpose. Needs to be documented, and made to work reliably and
tested
talk about extrinsics. I leave cam0 where it is, and move cam1.
default geometry selectable. baseline stable or fixed at 1.0?
We always leave the extrinsics of camera0, and move camera1 instead. By default,
we move camera1 as dictated by the solve, leaving the baseline the same if the
solve is unique up-to-scale only.
'''
import sys
import argparse
import re
import os
def parse_args():
def positive_int(string):
try:
value = int(string)
except:
raise argparse.ArgumentTypeError("argument MUST be a positive integer. Got '{}'".format(string))
if value <= 0 or abs(value-float(string)) > 1e-6:
raise argparse.ArgumentTypeError("argument MUST be a positive integer. Got '{}'".format(string))
return value
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--valid-intrinsics-region',
action='store_true',
help='''If given, annotate the image with its
valid-intrinsics region''')
parser.add_argument('--range-estimate',
type=float,
default=10.,
help='''Initial guess for the range of all picked
points. Defaults to 10m''')
parser.add_argument('--template-size',
type=positive_int,
nargs=2,
default = (13,13),
help='''The size of the template used for feature
matching, in pixel coordinates of the second image. Two
arguments are required: width height. This is passed
directly to mrcal.match_feature(). We default to
13x13''')
parser.add_argument('--search-radius',
type=positive_int,
default = 20,
help='''How far the feature-matching routine should
search, in pixel coordinates of the second image. This
should be larger if the nominal range estimate is poor,
especially, at near ranges. This is passed directly to
mrcal.match_feature(). We default to 20 pixels''')
parser.add_argument('--initial-correspondences', '--correspondences',
help='''Start with the given correspondences vnl. If
omitted, we don't start with any features at all. The
given vnl must have the columns: x0 y0 x1 y1. Exclusive
with --initial-features''')
parser.add_argument('--initial-features',
help='''Start with the given features vnl. If omitted,
we don't start with any features at all. At startup we
try to find matches in image1 for each of these
features; any failed matches are discarded. The given
vnl must have the columns: x0 y0. Exclusive with
--initial-correspondences''')
parser.add_argument('--rt01',
help='''We always leave the extrinsics of camera0, and
move camera1, as dictated by the solve; we leave the
baseline the same if the solve is unique up-to-scale
only. By default we start with the geometry given by the
models. If --rt01 is given, we use this relative initial
geometry instead. This is a comma-separated list of 6
numbers''')
######## image pre-filtering; same as in mrcal-stereo; please consolidate
parser.add_argument('--equalization',
choices=('clahe', 'fieldscale','stretch'),
help='''The equalization method to use for the input
images. "fieldscale" requires mrcam to be installed, and
can only operate on uint16 images''')
parser.add_argument('models',
type=str,
nargs = 2,
help='''Camera models representing cameras used to
capture the images. Intrinsics only are used. A nominal
stereo geometry with unit baseline is assumed. Given
models are the left, right cameras''')
parser.add_argument('images',
type=str,
nargs=2,
help='''The images to use for the matching''')
args = parser.parse_args()
if args.initial_correspondences is not None and \
args.initial_features is not None:
print("--initial-correspondences and --initial-features are mutually exclusive",
file=sys.stderr)
sys.exit(1)
return args
args = parse_args()
# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README
from fltk import *
from Fl_Gl_Image_Widget import *
import numpy as np
import numpysane as nps
import mrcal
import pyopengv
import vnlog
if args.equalization == 'fieldscale':
try:
import mrcam
except:
print("ERROR: the 'fieldscale' equalization method requires mrcam, but it could not be imported", file=sys.stderr)
sys.exit(1)
if args.equalization == 'clahe':
import cv2
def get_q01_estimate(*,
q,
index):
i_from = index
i_to = 1-index
if index == 0: Rt_to_from = mrcal.invert_Rt(context['Rt01'])
else: Rt_to_from = context['Rt01']
v_from = mrcal.unproject(q, *models[i_from].intrinsics(),
normalize = True)
p_from = v_from * args.range_estimate
p_to = mrcal.transform_point_Rt(Rt_to_from, p_from)
q_to = mrcal.project(p_to, *models[i_to].intrinsics())
if index == 0:
return nps.cat(q, q_to)
else:
return nps.cat(q_to, q)
def solve_R01__kneip(v0,v1,
**cookie):
r'''Rotation from corresponding-feature observations: Kneip's eigesolver
Kneip describes a method to compute the rotation:
L. Kneip, R. Siegwart, M. Pollefeys, "Finding the Exact Rotation Between Two
Images Independently of the Translation", Proc. of The European Conference on
Computer Vision (ECCV), Florence, Italy. October 2012.
L. Kneip, S. Lynen, "Direct Optimization of Frame-to-Frame Rotation", Proc. of
The International Conference on Computer Vision (ICCV), Sydney, Australia.
December 2013.
'''
if cookie:
raise Exception("This function does not use the cookie")
return \
pyopengv.relative_pose_eigensolver(v0,v1,
# seed
mrcal.identity_R())
def solve_R01__assume_infinity(v0,v1,
**cookie):
r'''Rotation from corresponding-feature observations: assume everything is infinitely-far away
For testing or maybe seeding
'''
# if cookie:
# raise Exception("This function does not use the cookie")
return \
mrcal.align_procrustes_vectors_R01(v0,v1)
def solve_R01__optimize(v0,v1,
*,
# cookie
t01_fixed = None):
r'''Rotation from corresponding-feature observations: direct optimization
This is for experimenting. solve_R01__kneip() should work well.
'''
import scipy.optimize
def cost__coplanarity_normal_vectors(r01):
n = np.cross(v0,
mrcal.rotate_point_r(r01,v1))
l,v = mrcal.sorted_eig(nps.matmult(n.T,n))
return l[0]
def cost__triangulation_geometric(r01):
R01 = mrcal.R_from_r(r01)
t01,Epredicted,A,B = t01_from_R01__geometric(v0,v1,
R01 = R01)
return Epredicted
def cost__triangulation_have_t01(r01):
R01 = mrcal.R_from_r(r01)
p0 = \
mrcal.triangulate_leecivera_wmid2(v0, v1,
v_are_local = True,
Rt01 = nps.glue(R01,
t01_fixed,
axis = -2))
# Have triangulated points p0. Divergent or at-infinity triangulations
# are reported as p0=(0,0,0).
#
# I want to minimize sum(th_err). It's analogous to maximize
# sum(cos(th_err)) ~ sum(inner(v0, p0/mag(p0))). I minimize
# -sum(inner(...)) instead.
#
# For the divergent points I assume we're at infinity, and use inner(v0,
# R01 v1)/2. The /2 is because the convergent cost is from v0 to the
# midpoint, and I want to keep the same scaling for the divergent cost.
# This inner(v0,v1) logic will treat noise incorrectly (just before
# infinity I correctly only respond to yaw noise, but past infinity I
# would respond to all noise). This is close-enough for now.
mask = nps.norm2(p0) > 0
if np.any(mask):
cost_convergent = \
-np.sum( nps.inner(p0[mask]/nps.dummy(nps.mag(p0[mask]),-1), v0[mask]) )
else:
cost_convergent = 0
if np.any(~mask):
cost_divergent = \
-np.sum( nps.inner(v0[~mask], mrcal.rotate_point_R(R01, v1[~mask])) ) / 2
else:
cost_divergent = 0
return cost_convergent + cost_divergent
if t01_fixed is None:
res = \
scipy.optimize.minimize( cost__triangulation_geometric,
x0 = mrcal.identity_r() )
else:
res = \
scipy.optimize.minimize( cost__triangulation_have_t01,
x0 = mrcal.identity_r() )
R01 = mrcal.R_from_r(res.x)
return R01
# debugging stuff
if True:
t01,Epredicted,A,B = t01_from_R01__geometric(v0,v1,
R01 = R01)
# e = p - k0 v0
# E = sum(norm2(e_i))
Rt01 = nps.glue(R01, t01, axis = -2)
Rt01neg = nps.glue(R01, -t01, axis = -2)
p = mrcal.triangulate_geometric(v0, v1,
v_are_local = True,
Rt01 = Rt01 )
E = np.sum(nps.norm2(p - nps.dummy(nps.inner(p,v0),-1) * v0))
print(f"E_relerr={(E-Epredicted) / ((np.abs(E)+np.abs(Epredicted))/2.)} {E=} {Epredicted=}")
R01_in = context['Rt01'][:3,:]
R01_procrustes = mrcal.align_procrustes_vectors_R01(v0,v1)
R01_kneip = solve_R01__kneip(v0,v1)
r01_in = mrcal.r_from_R(R01_in)
r01_procrustes = mrcal.r_from_R(R01_procrustes)
r01_kneip = mrcal.r_from_R(R01_kneip)
if False:
print(f"{r01=} {r01_in=} {r01_procrustes=} {r01_kneip=}")
return R01
def t01_from_R01__geometric(v0,v1,R01):
r'''Translation from corresponding-feature observations: Dima's method
Given corresponding feature observations from two cameras we compute the
transform between the cameras. This is unique up-to-scale, so the reported
translation has length = 1.
This method assumes a rotation is already available. Given this rotation this
method computes the translation. This may or may not be any better than
solve_Rt01_given_R01__epipolar_plane_normality(). That needs evaluation.
The derivation of this method follows. I use the geometric triangulation
expression derived here:
https://github.com/dkogan/mrcal/blob/8be76fc28278f8396c0d3b07dcaada2928f1aae0/triangulation.cc#L112
I assume that I'm triangulating normalized v0,v1 both expressed in cam-0
coordinates. And I have a t01 translation that I call "t" from here on. This is
unique only up-to-scale, so I assume that norm2(t) = 1. The geometric
triangulation from the above link says that
[k0] = 1/(v0.v0 v1.v1 -(v0.v1)**2) [ v1.v1 v0.v1][ t01.v0]
[k1] [ v0.v1 v0.v0][-t01.v1]
The midpoint p is
p = (k0 v0 + t01 + k1 v1)/2
I assume that v are normalized and I represent k as a vector. I also define
c = inner(v0,v1)
So
k = 1/(1 - c^2) [1 c] [ v0t] t
[c 1] [-v1t]
I define
A = 1/(1 - c^2) [1 c] This is a 2x2 array
[c 1]
B = [ v0t] This is a 2x3 array
[-v1t]
V = [v0 v1] This is a 3x2 array
Note that none of A,B,V depend on t.
So
k = A B t
Then
p = (k0 v0 + t01 + k1 v1)/2
= (V k + t)/2
= (V A B t + t)/2
= (I + V A B) t/2
Each triangulated error is
e = mag(p - k0 v0)
I split A into its rows
A = [ a0t ]
[ a1t ]
Then
e = p - k0 v0
= (I + V A B) t/2 - v0 a0t B t
= I t/2 + V A B t/2 - v0 a0t B t
= I t/2 + v0 a0t B t/2 + v1 a1t B t/2 - v0 a0t B t
= I t/2 - v0 a0t B t/2 + v1 a1t B t/2
= ((I - v0 a0t B + v1 a1t B) t) / 2
= ((I + (- v0 a0t + v1 a1t) B) t) / 2
= ((I - Bt A B) t) / 2
I define a joint error function I'm optimizing as the sum of all the individual
triangulation errors:
E = sum(norm2(e_i))
Each component is
norm2(e) = 1/4 tt (I - Bt A B)t (I - Bt A B) t
= 1/4 tt (I - 2 Bt A B + Bt A B Bt A B ) t
B Bt = [1 -c]
[-c 1]
B Bt A = 1/(1 - c^2) [1 -c] [1 c]
[-c 1] [c 1]
= 1/(1 - c^2) [1-c^2 0 ]
[0 1-c^2 ]
= I
-> norm2(e) = 1/4 tt (I - 2 Bt A B + Bt A B) t
= 1/4 tt (I - Bt A B) t
= 1/4 - 1/4 tt Bt A B t
So
E = N/4 - 1/4 tt sum(Bt A B) t
I let
M = sum(Bt A B)
M = sum(Mi)
Mi = Bt A B
So
E = N/4 - 1/4 tt M t
= N/4 - 1/4 lambda
So to minimize E I find t that is the eigenvector of M that corresponds to its
largest eigenvalue lambda. Furthermore, lambda depends on the rotation. If I
couldn't estimate the rotation from far-away features I can solve the
eigenvalue-optimization problem to maximize lambda.
More simplification:
Mi = Bt A B = [ v0 -v1 ] A [ v0t]
[-v1t]
= 1/(1-c^2) [ v0 - v1 c v0 c - v1] [ v0t]
[-v1t]
= 1/(1-c^2) ((v0 - v1 c) v0t - (v0 c - v1) v1t)
c = v0t v1 ->
F0 = v0 v0t
F1 = v1 v1t
-> Mi = 1/(1-c^2) (v0 v0t - v1 v1t v0 v0t + v1 v1t - v0 v0t v1 v1t)
= 1/(1-c^2) (F0 + F1 - (F1 F0 + F0 F1))
= (F0 - F1)^2 / (1 - c^2)
= ((F0 - F1)/s)^2
where s = mag(cross(v0,v1))
tt M t = sum( norm2((F0i - F1i)/si t) )
Let Di = (F0i - F1i)/si
I want to maximize sum( norm2(Di t) )
(F0 - F1)/s = (v0 v0t - v1 v1t) / mag(cross(v0,v1))
~ (v0 v0t - R v1 v1t Rt) / mag(cross(v0,R v1))
experiments:
= 1/(1-c^2) (F0 + F1 - (F1 F0 + F0 F1))
= 1/(1-c^2) (v0 v0t + v1 v1t - (c v0 v1t + c v1 v0t))
'''
v1 = mrcal.rotate_point_R(R01,v1)
# shape (N,)
c = nps.inner(v0,v1)
N = len(c)
# shape (N,2,2)
A = np.ones((N,2,2), dtype=float)
A[:,0,1] = c
A[:,1,0] = c
A /= nps.mv(1. - c*c, -1,-3)
# shape (N,2,3)
B = np.empty((N,2,3), dtype=float)
B[:,0,:] = v0
B[:,1,:] = -v1
# shape (3,3)
M = np.sum( nps.matmult(nps.transpose(B), A, B), axis = 0 )
l,v = mrcal.sorted_eig(M)
# The answer is the eigenvector corresponding to the biggest eigenvalue
t01 = v[:,-1]
E = N/4 - l[-1]/4
return t01,E,A,B
def solve_Rt01_given_R01__geometric(v0,v1,R01):
r'''Estimate pose from rotation and feature observations
See the docstring for t01_from_R01__geometric()
'''
t01,Epredicted,A,B = t01_from_R01__geometric(v0,v1,R01)
# Almost done. I want either t or -t. The wrong one will produce
# mostly triangulations behind me
k = nps.matmult(A,B, nps.transpose(t01))[..., 0]
mask_divergent_t = (k[:,0] <= 0) + (k[:,1] <= 0)
mask_divergent_negt = (k[:,0] >= 0) + (k[:,1] >= 0)
N_divergent_t = np.count_nonzero( mask_divergent_t )
N_divergent_negt = np.count_nonzero( mask_divergent_negt )
if False:
# Epipolar plane normals. Should be planar, if R01 is right
n = np.cross(v0,mrcal.rotate_point_R(R01,v1))
l,vv = mrcal.sorted_eig(nps.matmult(n.T,n))
print(f"epipolar normal t,mine: {vv[:,0]=} {t01=}")
if N_divergent_t < N_divergent_negt:
return ( nps.glue(R01, t01, axis=-2),
mask_divergent_t,
N_divergent_t )
else:
return ( nps.glue(R01, -t01, axis=-2),
mask_divergent_negt,
N_divergent_negt )
def solve_Rt01_given_R01__epipolar_plane_normality(v0,v1,R01):
r'''Translation from corresponding-feature observations: Kneip's method
Given corresponding feature observations from two cameras we compute the
transform between the cameras. This is unique up-to-scale, so the reported
translation has length = 1.
This method assumes a rotation is already available (from Kneip's eigensolver,
for instance). Given this rotation this method computes the translation. This
may or may not be any better than solve_Rt01_given_R01__geometric(). That needs
evaluation.
Kneip describes a method to compute the ROTATION:
L. Kneip, R. Siegwart, M. Pollefeys, "Finding the Exact Rotation Between Two
Images Independently of the Translation", Proc. of The European Conference on
Computer Vision (ECCV), Florence, Italy. October 2012.
L. Kneip, S. Lynen, "Direct Optimization of Frame-to-Frame Rotation", Proc. of
The International Conference on Computer Vision (ICCV), Sydney, Australia.
December 2013.
The rotation is the hard part, but we do still need a translation. opengv should
do this for us, and I think its C++ API does, but its Python bindings are
lacking. So I compute t myself for now
'''
Rt01 = np.zeros((4,3), dtype=float)
Rt01[:3,:] = R01
# shape (N,3)
c = np.cross(v0, mrcal.rotate_point_R(R01, v1))
l,v = mrcal.sorted_eig(np.sum(nps.outer(c,c), axis=0))
# t is the eigenvector corresponding to the smallest eigenvalue
t01 = v[:,0]
Rt01[3,:] = t01
# Almost done. I want either t or -t. The wrong one will produce
# mostly triangulations behind me
p_t = mrcal.triangulate_geometric(v0, v1,
v_are_local = True,
Rt01 = Rt01 )
mask_divergent_t = (nps.norm2(p_t) == 0)
N_divergent_t = np.count_nonzero( mask_divergent_t )
Rt01_negt = Rt01 * nps.transpose(np.array((1,1,1,-1),))
p_negt = mrcal.triangulate_geometric(v0, v1,
v_are_local = True,
Rt01 = Rt01_negt )
mask_divergent_negt = (nps.norm2(p_negt) == 0)
N_divergent_negt = np.count_nonzero( mask_divergent_negt )
if N_divergent_t == 0 and N_divergent_negt == 0:
raise Exception("Cannot ambiguously determine t: neither has divergent observations")
# We have divergences even in the best case. I pick the fewer-divergence
# case, and report the diverging observations as outliers. All converging
# observations are treated as inlier here, regardless of reprojection error
if N_divergent_t < N_divergent_negt:
return (Rt01,
mask_divergent_t,
N_divergent_t)
else:
return (Rt01_negt,
mask_divergent_negt,
N_divergent_negt)
def solve_Rt01__baseline_noz(v0,v1,R01,
*,
t01_fixed = None,
baseline_want = None,
th0 = None,
dth = None,
Nth = None):
r'''Translation keeping a constant z offset, sampling the xy plane
'''
if t01_fixed is not None:
raise Exception("This function uses baseline_want, and NOT t01_fixed")
if baseline_want is None:
raise Exception("This function requires baseline_want")
# I do this discretely
if not ( th0 is not None \
and dth is not None \
and Nth is not None):
# default: look at the full 360 span
Nth = 1000
thdeg = np.linspace(0, 360.,
num = Nth,
endpoint = False)
else:
thdeg = np.linspace(th0 - dth,
th0 + dth,
num = Nth)
th = thdeg * np.pi/180.
c = np.cos(th)
s = np.sin(th)
Rt01 = np.zeros((Nth,4,3), dtype=float)
Rt01[:,:3,:] += R01
Rt01[:, 3,:] = nps.transpose( nps.cat(c, s, np.zeros(c.shape)) ) * baseline_want
# shape (Nth,Npoints,3)
p = mrcal.triangulate_geometric(v0, v1,
v_are_local = True,
Rt01 = nps.dummy(Rt01, -3) )
Npoints = p.shape[-2]
# shape (Nth,Npoints)
magp = nps.mag(p)
# shape (Nth,Npoints)
mask_divergent = (magp == 0)
# shape (Nth)
N_divergent = np.count_nonzero( mask_divergent,
axis = -1)
mean_thsq_err = np.zeros( (Nth,), dtype=float)
mean_thsq_err_accept_divergences = np.zeros( (Nth,), dtype=float)
for ith in range(Nth):
m = ~mask_divergent[ith]
if not np.any(m):
mean_thsq_err[ith] = 1e6
else:
# inner ~ cos ~ 1 - th^2/2
sum_cos = np.sum(nps.inner(p[ith][m], v0[m]) / magp[ith][m])
mean_cos = sum_cos / np.count_nonzero(m)
sum_cos_accept_divergences = sum_cos + np.count_nonzero(~m)
mean_cos_accept_divergences = sum_cos_accept_divergences/Npoints
mean_thsq_err[ith] = 2.*(1. - mean_cos)
mean_thsq_err_accept_divergences[ith] = 2.*(1. - mean_cos_accept_divergences)
# diagnostic code to visualize the scores and divergence counts
if False:
import gnuplotlib as gp
gp.plot( (th/np.pi*180., nps.cat(mean_thsq_err,
mean_thsq_err_accept_divergences), ),
(th/np.pi*180., N_divergent,
dict(y2=True) ),
ymax=1e-6)
import IPython
IPython.embed()
sys.exit()
ith = np.argmin(mean_thsq_err)
if False:
# be truthful about divergences
return (Rt01[ith],
mask_divergent[ith],
N_divergent[ith])
else:
# lie, and say that there were no divergences
return (Rt01[ith],
np.zeros( (Npoints,), dtype=bool),
0)
def solve_Rt10(# shape (N,Nleftright=2,Nxy=2)
q01,
intrinsics0,
intrinsics1,
*,
solve_R10 = solve_R01__kneip,
solve_Rt10_from_R01 = solve_Rt01_given_R01__geometric,
# cookie
t01_fixed = None,
baseline_want = None):
r'''Full-transform optimization
Given corresponding feature observations from two cameras we compute the
transform between the cameras. This is unique up-to-scale, so the reported
translation has length = 1.
The methods to compute the rotation and the translation are given in solve_R10
and solve_Rt10_from_R01.
'''
# shape (N,3)
# These are in their LOCAL coord system
v0 = mrcal.unproject(q01[...,0,:], *intrinsics0, normalize = True)
v1 = mrcal.unproject(q01[...,1,:], *intrinsics1, normalize = True)
# Keep all all points initially
mask_inliers = np.ones( (q01.shape[0],), dtype=bool )
while True:
if np.count_nonzero(mask_inliers) == 0:
raise Exception("All points thrown out as outliers")
R01 = solve_R10(v0[mask_inliers],
v1[mask_inliers],
t01_fixed = t01_fixed)
if t01_fixed is not None:
Rt01 = nps.glue(R01, t01_fixed, axis=-2)
return Rt01, mask_inliers
Rt01, mask_outlier, Noutliers = \
solve_Rt10_from_R01(v0[mask_inliers],
v1[mask_inliers],
R01 = R01,
t01_fixed = t01_fixed,
baseline_want = baseline_want)
if Noutliers == 0:
break
mask_inliers[np.nonzero(mask_inliers)[0][mask_outlier]] = False
# This hopefully was already enforced above
# if baseline_want is not None:
# baseline_have = nps.mag(Rt01[3,:])
# Rt01[3,:] *= baseline_want/baseline_have
return \
Rt01, \
mask_inliers
def update_p0_triangulated_stored():
global context
q0 = context['q01_stored'][...,0,:]
q1 = context['q01_stored'][...,1,:]
v0 = mrcal.unproject(q0, *models[0].intrinsics(), normalize = True)
v1 = mrcal.unproject(q1, *models[1].intrinsics(), normalize = True)
context['p0_triangulated_stored'] = \
mrcal.triangulate_geometric(v0, v1,
v_are_local = True,
Rt01 = context['Rt01'] )
def update_q01_stored(_q01_stored):
global context
context['q01_stored'] = _q01_stored
update_p0_triangulated_stored()
try:
widget_table.rows(len(context['q01_stored']))
except NameError:
# widget_table might not exist yet
pass
def line_segments_squares(# shape (..., 2)
q,
radii):
Nradii = len(radii)
# shape (..., Nradii, Nsegments_in_square=4, Npoints_in_line_segment=2, xy=2)
p = np.zeros(q.shape[:-1] + (Nradii,4,2,2), dtype=np.float32)
p += nps.dummy(q,-2,-2,-2)
rx = np.array((1,0), dtype=np.float32)
ry = np.array((0,1), dtype=np.float32)
for i,radius in enumerate(radii):
# line segment 0
p[..., i,0,0,:] += (-rx-ry)*radius
p[..., i,0,1,:] += (-rx+ry)*radius
# line segment 1
p[..., i,1,0,:] += (+rx-ry)*radius
p[..., i,1,1,:] += (+rx+ry)*radius
# line segment 2
p[..., i,2,0,:] += (-rx-ry)*radius
p[..., i,2,1,:] += (+rx-ry)*radius
# line segment 3
p[..., i,3,0,:] += (-rx+ry)*radius
p[..., i,3,1,:] += (+rx+ry)*radius
# flatten to a list of line segments
# shape (N, 2,2)
return nps.clump(p, n=p.ndim-2)
def line_segments_crosshairs(# shape (..., 2)
q,
radius):
# shape (..., Nsegments_in_crosshair=2, Npoints_in_line_segment=2, xy=2)
p = np.zeros(q.shape[:-1] + (4,2,2), dtype=np.float32)
p += nps.dummy(q,-2,-2)
rx = np.array((1,0), dtype=np.float32)
ry = np.array((0,1), dtype=np.float32)
# line segment 0
p[..., 0,0,:] += (-rx-ry)*radius
p[..., 0,1,:] += (+rx+ry)*radius
# line segment 1
p[..., 1,0,:] += (+rx-ry)*radius
p[..., 1,1,:] += (-rx+ry)*radius
# flatten to a list of line segments
# shape (..., 2,2)
return nps.clump(p, n=p.ndim-2)
def set_all_overlay_lines_and_redraw():
radius_stored = 10
crosshair_radius = 1
color_stored = np.array((0,1,0), dtype=np.float32)
radius_selected = 11
color_selected = np.array((0,0,1), dtype=np.float32)
color_searchbox = np.array((1,1,0), dtype=np.float32)
Nstored = len(context['q01_stored'])
Rt01 = context['Rt01']
baseline = nps.mag(Rt01[3,:])
# for projected rays, I start at a small ratio of the baseline, and increase
# the range geometrically. I add another point at infinity at the end
k = 1.1
N = 25
r = baseline * 1. * (k ** np.arange(N))
iselected = tuple(i for i in range(Nstored) if widget_table.row_selected(i))
for w in widgets_image:
####### The squares and crosshair for all the stored features
lines_stored = \
[ dict(points = nps.glue( line_segments_squares(context['q01_stored'][:, w.index,:],
(radius_stored,)),
line_segments_crosshairs(context['q01_stored'][:, w.index,:],
crosshair_radius),
axis = -3 ),
color_rgb = color_stored ) ]
####### The search box
if context['search_center__q01_indexfrom_showfrom_searchradius'] is not None:
q01_search_center,indexfrom,showfrom,search_radius = context['search_center__q01_indexfrom_showfrom_searchradius']
if indexfrom == w.index and showfrom:
lines_stored.append( dict(points = line_segments_crosshairs(q01_search_center[w.index],
crosshair_radius),
color_rgb = color_searchbox ) )
elif indexfrom != w.index:
lines_stored.append( dict(points = line_segments_squares(q01_search_center[w.index],
(search_radius,)),
color_rgb = color_searchbox ) )
if len(iselected):
####### The epipolar CURVE corresponding to THIS point in the OTHER
####### camera
# shape (Nselected, 2)
q_stored = context['q01_stored'][iselected, w.index,:]
q_stored_other = context['q01_stored'][iselected, 1-w.index,:]
# shape (Nranges, Nselected, 3)
p = mrcal.unproject(q_stored_other, *models[1-w.index].intrinsics(),
normalize = True) * \
nps.mv(r,-1,-3)
if w.index == 0:
pinf = mrcal.rotate_point_R(Rt01[:3,:], p[0])
p = mrcal.transform_point_Rt(Rt01, p)
else:
pinf = mrcal.rotate_point_R(mrcal.invert_R(Rt01[:3,:]), p[0])
p = mrcal.transform_point_Rt(mrcal.invert_Rt(Rt01), p)
# shape (Nsegments, Nsegmentpoint=2, Nxy=2)
qsegments = np.zeros( (0,2,2), dtype=np.float32)
# shape (Nranges+1, Nselected, Nxyz=3)
p = nps.glue(p,pinf, axis=-3)
for pselected in nps.mv(p,-2,-3):
# pselected.shape is (Nranges+1,Nxyz=3)
# cut off points where the ray is behind the other camera
pselected = pselected[pselected[:,2] > 0,:]
if len(pselected)>=2:
# shape (N, Nxy=2)
q = mrcal.project(pselected,
*models[w.index].intrinsics()).astype(np.float32)
# shape (N-1, Nsegmentpoint=2, Nxy=2)
qsegments = \
nps.glue(qsegments,
nps.mv( nps.cat(q[0:-1,...],q[1:,...]),
0,1 ),
axis=-3)
lines_stored.append( dict(points = nps.glue( line_segments_squares(q_stored,
(radius_selected,),),
qsegments,
axis = -3),
color_rgb = color_selected ) )
w.set_lines(*lines_stored)
def clear_q_search_center():
global context
context['search_center__q01_indexfrom_showfrom_searchradius'] = None
def widget_table_callback(*args):
ctx = widget_table.callback_context()
event = Fl.event()
if ctx == Fl_Table.CONTEXT_CELL and \
event == FL_RELEASE:
# I want the selected feature to be in view. I do that for image0
# (image1 will be automatically panned). If it's already in view or
# close to it, then I don't touch it
row = widget_table.callback_row()
i_image = 0
q = context['q01_stored'][row,i_image,:]
viewport_w = widgets_image[i_image].w()
viewport_h = widgets_image[i_image].h()
qviewport = widgets_image[i_image].map_pixel_viewport_from_image(q)
# The margin is 1/10 or 100 pixels, whichever is smaller. If we're within
# the margin, we pan
margin_w = min(viewport_w//10,100)
margin_h = min(viewport_h//10,100)
if qviewport[0] < margin_w or viewport_w-1-qviewport[0] < margin_w or \
qviewport[1] < margin_h or viewport_h-1-qviewport[1] < margin_h:
widgets_image[i_image].set_panzoom(*q,
visible_width_pixels = np.nan, # leave the same
notify_other_widgets = True)
clear_q_search_center()
set_all_overlay_lines_and_redraw()
def widget_solve_callback(*args):
global context
try:
Rt01, mask_inliers = \
solve_Rt10(context['q01_stored'],
*[m.intrinsics() for m in models],
solve_R10 = solve_R01__optimize,
t01_fixed = context['Rt01'][3,:])
except Exception as e:
widget_info.value(UI_usage_message + "\n" + \
f"Solve failed: {e}")
return
# Store the new transform. The model0 extrinsics are untouched, but I move
# model1
context['Rt01'] = Rt01
Rtr0 = models[0].Rt_ref_cam()
Rtr1 = mrcal.compose_Rt(Rtr0, Rt01)
models[1].Rt_ref_cam(Rtr1)
update_p0_triangulated_stored()
widget_table.redraw()
clear_q_search_center()
set_all_overlay_lines_and_redraw()
N = len(context['q01_stored'])
Noutliers = N-np.count_nonzero(mask_inliers)
widget_info.value(UI_usage_message + "\n" + \
f"Extrinsics of model1 updated: {Noutliers}/{N} outliers\n" + \
f"Outlier points: {np.nonzero(~mask_inliers)[0]}")
# Debugging code to write the models and correspondences to disk after each
# solve
if False:
for icam in (0,1):
filename = f"/tmp/camera-{icam}.cameramodel"
models[icam].write(filename)
print(f"Wrote '{filename}")
p = context['p0_triangulated_stored']
r = nps.mag(p)
idx = nps.norm2(p) > 0
r[~idx] = np.nan
filename = '/tmp/correspondences.vnl'
np.savetxt(filename,
nps.glue( nps.clump(context['q01_stored'][:,:4],
n = -2),
nps.dummy(r,-1),
axis = -1),
fmt = '%.2f',
header = "x0 y0 x1 y1 range")
print(f"Wrote '{filename}")
def widget_write_callback(*args):
p = context['p0_triangulated_stored']
r = nps.mag(p)
idx = nps.norm2(p) > 0
r[~idx] = np.nan
np.savetxt(sys.stdout,
nps.glue( nps.clump(context['q01_stored'][:,:4],
n = -2),
nps.dummy(r,-1),
axis = -1),
fmt = '%.2f',
header = "x0 y0 x1 y1 range")
for m in models: m.write(sys.stdout)
def find_corresponding_feature(q, index,
*,
qother_estimate = None,
search_radius):
if not (q[0] >= -0.5 and q[0] <= W-0.5 and \
q[1] >= -0.5 and q[1] <= H-0.5):
return f"Out of bounds: {q=}", None, None
if qother_estimate is None:
# shape (2,2): (leftright, qxy)
q01_estimate = get_q01_estimate(q = q,
index = index)
else:
if index == 0: q01_estimate = nps.cat(q, qother_estimate)
else: q01_estimate = nps.cat(qother_estimate,q)
index_other = 1 - index
try:
match_feature_out = \
mrcal.match_feature(images[index], images[index_other],
q0 = q01_estimate[index],
q1_estimate = q01_estimate[index_other],
search_radius1 = search_radius,
template_size1 = args.template_size)
q_other, match_feature_diagnostics = match_feature_out[:2]
except:
q_other = None
if q_other is None:
return "Error matching feature", q01_estimate, None
q01 = np.array(q01_estimate)
q01[index_other] = q_other
return "Feature match successful", q01_estimate, q01
class Fl_Gl_Image_Widget_Derived(Fl_Gl_Image_Widget):
def __init__(self,
*args,
index,
**kwargs):
self.index = index
return super().__init__(*args, **kwargs)
def set_panzoom(self,
x_centerpixel, y_centerpixel,
visible_width_pixels,
notify_other_widgets = True):
r'''Pan/zoom the image
This is an override of the function to do this: any request to
pan/zoom the widget will come here first. I dispatch any
pan/zoom commands to all the widgets, so that they all work in
unison. visible_width_pixels < 0 means: this is the redirected
call; just call the base class
'''
if not notify_other_widgets:
return super().set_panzoom(x_centerpixel, y_centerpixel,
visible_width_pixels)
# All the widgets should pan/zoom together
result = \
all( w.set_panzoom(x_centerpixel, y_centerpixel,
visible_width_pixels,
notify_other_widgets = False) \
for w in widgets_image )
# Switch back to THIS widget
self.make_current()
return result
def handle_right_mouse_button(self):
search_center__q01_indexfrom_showfrom_searchradius_copy = context['search_center__q01_indexfrom_showfrom_searchradius']
clear_q_search_center()
try:
q = \
np.array( self.map_pixel_image_from_viewport( (Fl.event_x(),Fl.event_y()), ),
dtype=float )
except:
q = None
if q is None:
message = "Error converting pixel coordinates"
widget_info.value(UI_usage_message + "\n" + \
message)
return 1
if Fl.event_state() & FL_CTRL:
if search_center__q01_indexfrom_showfrom_searchradius_copy is None:
# Ctrl-right-click but we didn't just do a search. Do nothing
return 1
# We just did a search. It may have succeeded, with the match
# selected, in the last row of the table. Or it may have failed,
# with nothing selected
q01_search_center, indexfrom, showfrom, _ = search_center__q01_indexfrom_showfrom_searchradius_copy
if self.index == indexfrom:
# Ctrl-right-click but we just did a search, from the same image. Do nothing
return 1
# We can rerun the search with human seeding. If the search we just
# did succeeded, then I need to clear out that feature; I'm about to
# redo it
iselected = tuple(i for i in range(widget_table.rows()) if widget_table.row_selected(i))
if len(iselected) == 0:
# Nothing selected; the previous search failed, and we don't
# need to clear it out
pass
elif len(iselected) > 1:
print("WARNING: We're trying to rerun a search, but more than one feature is selected in the UI. This shouldn't be able to happen, and it's a bug. Doing nothing")
return 1
else:
# Have one feature. Delete it
mask = np.ones((widget_table.rows(),), dtype=bool)
mask[iselected] = 0
update_q01_stored(context['q01_stored'][mask, ...])
widget_table.rows(len(context['q01_stored']))
widget_table.select_all_rows(0) # deselect all
# Set up the new search. I want to search FROM the other widget: I'm
# repeating the previous search
qother_estimate = q
q = q01_search_center[indexfrom]
search_radius = 5 # tighter bounds
else:
# normal path
indexfrom = self.index
qother_estimate = None
search_radius = args.search_radius
message,q01_estimate,q01 = find_corresponding_feature(q, indexfrom,
qother_estimate = qother_estimate,
search_radius = search_radius)
widget_info.value(UI_usage_message + "\n" + \
message)
if q01 is not None:
update_q01_stored(nps.glue( context['q01_stored'],
q01,
axis=-3))
Nstored = len(context['q01_stored'])
widget_table.rows( Nstored )
widget_table.select_all_rows(0) # deselect all
widget_table.select_row(Nstored-1)
else:
# match failed
widget_table.select_all_rows(0) # deselect all
if q01_estimate is not None:
context['search_center__q01_indexfrom_showfrom_searchradius'] = (q01_estimate, indexfrom, q01 is None, search_radius)
return 1
def handle(self, event):
if event == FL_ENTER:
return 1
if event == FL_LEAVE:
widget_status.value("")
return 1
if event == FL_MOVE:
try:
q = self.map_pixel_image_from_viewport( (Fl.event_x(),Fl.event_y()), )
this = f"Image {self.index}"
widget_status.value(f"{this}: {q[0]:.2f},{q[1]:.2f}")
except:
widget_status.value("")
return 1
if event == FL_PUSH and Fl.event_button() == FL_LEFT_MOUSE:
self.dragged = False
return super().handle(event)
if event == FL_DRAG and Fl.event_state() & FL_BUTTON1:
self.dragged = True
return super().handle(event)
if event == FL_RELEASE:
if Fl.event_button() == FL_RIGHT_MOUSE:
result = self.handle_right_mouse_button()
set_all_overlay_lines_and_redraw()
return result
if Fl.event_button() == FL_LEFT_MOUSE:
if not self.dragged:
clear_q_search_center()
# Clicked. Select the nearest feature
if len(context['q01_stored']) > 0:
qwidget = np.array((Fl.event_x(),Fl.event_y()),
dtype=int)
qwidget_stored = \
np.array([self.map_pixel_viewport_from_image(q) for q in context['q01_stored'][:,self.index,:]])
# within 10 pixels in the UI
i = np.nonzero(nps.norm2(qwidget - qwidget_stored) < 10*10)[0]
if len(i) < 1:
widget_info.value(UI_usage_message + "\n" + \
"No feature near click")
widget_table.select_all_rows(0) # deselect all
set_all_overlay_lines_and_redraw()
elif len(i) > 1:
widget_info.value(UI_usage_message + "\n" + \
"Too many features near click")
widget_table.select_all_rows(0) # deselect all
set_all_overlay_lines_and_redraw()
else:
# Just one feature. Select it
i = i[0]
widget_table.select_all_rows(0) # deselect all
widget_table.select_row(int(i))
set_all_overlay_lines_and_redraw()
return super().handle(event)
# Must be key UP, not key DOWN. Because of https://github.com/fltk/fltk/issues/1044
if event == FL_KEYUP:
if Fl.event_key() == fltk.FL_Delete:
i_keep = tuple(i for i in range(widget_table.rows()) \
if not widget_table.row_selected(i))
update_q01_stored(context['q01_stored'][i_keep, ...])
widget_table.rows(len(context['q01_stored']))
widget_table.select_all_rows(0) # deselect all
clear_q_search_center()
set_all_overlay_lines_and_redraw()
widget_info.value(UI_usage_message + "\n" + \
"Feature(s) deleted")
return 1
return super().handle(event)
class Fl_Table_Derived(Fl_Table_Row):
def __init__(self, x, y, w, h, *args):
Fl_Table_Row.__init__(self, x, y, w, h, *args)
self.col_labels = \
[ "x0",
"y0",
"x1",
"y1",
"Triangulated range",
"Cam0 triangulated reprojection error", ]
len_col_labels = [len(x) for x in self.col_labels]
min_len_col_labels = min(len_col_labels)
max_ratio_len_col_labels = 3 # limit max ratio
normalized_len_col_labels = [min(x/min_len_col_labels, max_ratio_len_col_labels) \
for x in len_col_labels]
sum_normalized_len_col_labels = sum(normalized_len_col_labels)
self.ratio_col_width = [w/sum_normalized_len_col_labels for w in normalized_len_col_labels]
self.type(fltk.Fl_Table_Row.SELECT_MULTI)
self.rows(len(context['q01_stored']))
self.cols(len(self.col_labels))
self.col_header(1)
self.col_resize(0)
self.when(FL_WHEN_RELEASE)
self.callback(widget_table_callback)
self.end()
def draw_cell(self, context_table, row, col, x, y, w, h):
if context_table == self.CONTEXT_STARTPAGE:
fl_font(FL_HELVETICA, 12)
return
if context_table == self.CONTEXT_COL_HEADER:
text = self.col_labels[col]
fl_push_clip(x, y, w, h)
fl_draw_box(FL_THIN_UP_BOX, x, y, w, h, self.row_header_color())
fl_color(FL_BLACK)
fl_draw(text, x, y, w, h, FL_ALIGN_CENTER)
fl_pop_clip()
return
if context_table == self.CONTEXT_CELL:
if col < 4:
iimage = col // 2
ixy = col % 2
text = f"{context['q01_stored'][row,iimage,ixy]:.2f}"
else:
p = context['p0_triangulated_stored'][row]
if nps.norm2(p) == 0:
# Divergent feature. No triangulated anything available
text = '-'
else:
if col == 4:
# range
text = f"{nps.mag(p):.2f}"
else:
# reprojection error
icam = 0
q = mrcal.project(p, *models[icam].intrinsics())
text = f"{nps.mag(context['q01_stored'][row,icam] - q):.1f}"
fl_push_clip(x, y, w, h)
# background color
fl_color(self.selection_color() if self.row_selected(row) else FL_WHITE)
fl_rectf(x, y, w, h)
# text
fl_color(FL_BLACK)
fl_draw(text, x, y, w, h, FL_ALIGN_CENTER)
# border
fl_color(FL_LIGHT2)
fl_rect(x, y, w, h)
fl_pop_clip()
return
return
def resize(self, x,y,w,h):
Fl_Table_Row.resize(self, x,y,w,h)
Ncols = self.cols()
width = self.w()
width_margin = 2 # for some reason, I need to cut this many pixels to
# avoid creating a scrollbar
x0 = 0
for icol in range(Ncols-1):
w_here = int(self.ratio_col_width[icol] * width)
self.col_width(icol, w_here)
x0 += w_here
# last col
self.col_width(Ncols-1, width-x0 - width_margin)
models = [mrcal.cameramodel(m) for m in args.models]
if args.rt01 is not None:
try:
rt01 = np.array([float(d) for d in args.rt01.split(',')])
except:
print("--rt01 must be a comma-separated list of 6 numbers; couldn't parse as numbers into an array", file=sys.stderr)
sys.exit(1)
if rt01.shape != (6,):
print("--rt01 must be a comma-separated list of 6 numbers; incorrect number of values given", file=sys.stderr)
sys.exit(1)
models[1].rt_ref_cam( mrcal.compose_rt(models[0].rt_ref_cam(),
rt01) )
# To make it possible to concisely print out the models
for m in models:
m.optimization_inputs_reset()
W,H = models[0].imagersize()
if args.equalization == 'clahe':
clahe = cv2.createCLAHE()
clahe.setClipLimit(8)
images = [mrcal.load_image(f) for f in args.images]
for i in range(2):
if images[i].ndim == 3:
images[i] = np.mean(images[i], axis=-1, dtype=images[i].dtype)
if args.equalization == 'clahe':
images = [ clahe.apply(image) for image in images ]
elif args.equalization == 'fieldscale':
images = [ mrcam.equalize_fieldscale(image) for image in images ]
elif args.equalization == 'stretch':
images = [ ((image - np.min(image))/(np.max(image)-np.min(image))*255.).astype(np.uint8) for image in images ]
if args.valid_intrinsics_region:
for i in range(2):
mrcal.annotate_image__valid_intrinsics_region(images[i], models[i])
# To make it possible to concisely print out the models
for m in models:
m.valid_intrinsics_region( (), )
UI_usage_message = r'''Usage:
Left mouse button click/drag: pan
Mouse wheel up/down/left/right: pan
Ctrl-mouse wheel up/down: zoom
'u': reset view: zoom out, pan to the center
Right-click: Find matching feature
Ctrl-right-click immediately after a right-click in the other widget: re-run
feature search starting with the new click position as a seed, and with a much
smaller search radius
Click in table: select feature(s)
Delete: delete selected feature(s)
'''
## these two arrays correspond to each other. If you update one, do update the
## other
context = dict( # shape (N, Nimages = 2,xy=2)
q01_stored = np.zeros((0,2,2), dtype=float),
# If non-None, describes the feature search that just occurred
search_center__q01_indexfrom_showfrom_searchradius = None,
# shape (N, xy=2)
p0_triangulated_stored = np.zeros((0,2), dtype=float),
Rt01 = mrcal.compose_Rt( models[0].Rt_cam_ref(),
models[1].Rt_ref_cam() ) )
if args.initial_correspondences is not None:
try:
context['q01_stored'] = vnlog.slurp(args.initial_correspondences,
dtype = np.dtype([('x0 y0 x1 y1', float, (4,))]))
except:
print(f"--initial-correspondences expects a text table with columns 'x0 y0 x1 y1'; couldn't load text table from '{args.initial_correspondences}'", file=sys.stderr)
sys.exit(1)
N = len(context['q01_stored'])
update_q01_stored(context['q01_stored']['x0 y0 x1 y1'].reshape((N,2,2),))
WINDOW_W = 800
IMAGES_H = 300
TABLE_H = 300
BUTTON_H = 30
STATUS_H = 20
WINDOW_H = IMAGES_H + TABLE_H + BUTTON_H + STATUS_H
window = Fl_Window(WINDOW_W, WINDOW_H, "mrcal feature picker")
body = Fl_Group(0,0,
WINDOW_W, IMAGES_H + TABLE_H)
y = 0
widgets_image = (Fl_Gl_Image_Widget_Derived(0, y,
WINDOW_W//2,IMAGES_H,
index = 0),
Fl_Gl_Image_Widget_Derived(WINDOW_W//2, y,
WINDOW_W//2,IMAGES_H,
index = 1))
y += IMAGES_H
widget_table = Fl_Table_Derived( 0, y,
WINDOW_W//2,TABLE_H)
widget_info = Fl_Multiline_Output(WINDOW_W//2,y,
WINDOW_W//2,TABLE_H)
y += TABLE_H
body.end()
Nbuttons = 2
BUTTON_W = WINDOW_W//Nbuttons
ibutton = 0
widget_solve_button = Fl_Button(ibutton*BUTTON_W, y,
BUTTON_W if ibutton < Nbuttons-1 else WINDOW_W-ibutton*BUTTON_W,
BUTTON_H, "Solve for the extrinsics")
widget_solve_button.callback(widget_solve_callback)
ibutton+=1
widget_write_button = Fl_Button(ibutton*BUTTON_W, y,
BUTTON_W if ibutton < Nbuttons-1 else WINDOW_W-ibutton*BUTTON_W,
BUTTON_H, "Write output to stdout")
widget_write_button.callback(widget_write_callback)
ibutton+=1
y += BUTTON_H
widget_status = Fl_Output(0,y,
WINDOW_W,STATUS_H)
widget_info.value(UI_usage_message)
y += STATUS_H
window.resizable(body)
window.end()
window.show()
for i in range(2):
widgets_image[i]. \
update_image(decimation_level = 0,
image_data = images[i])
if args.initial_features is not None:
try:
q0 = vnlog.slurp(args.initial_features,
dtype = np.dtype([('x0 y0', float, (2,))]))
except:
print(f"--initial-features expects a text table with columns 'x0 y0'; couldn't load text table from '{args.initial_features}'", file=sys.stderr)
sys.exit(1)
q0 = q0['x0 y0']
N = len(q0)
mask_good = np.zeros((N,), dtype=bool)
q01 = np.zeros( (N,2,2), dtype=float)
for i in range(N):
message,q01_estimate,_q01 = \
find_corresponding_feature(q0[i,:], 0,
search_radius = args.search_radius)
if _q01 is not None:
q01[i] = _q01
mask_good[i] = True
update_q01_stored(q01[mask_good])
if len(context['q01_stored']):
# Drawing lines at startup requires gl-image-display 0.19, so I only do it
# if I need to. Most of the time we will start out with no features, so
# older gl-image-display would be fine
set_all_overlay_lines_and_redraw()
Fl.run()
sys.exit(0)
r'''
todo notes:
'u' should pan to the center
ctrl-drag should lock panning
arrow-keys in the table should work
better UI messages
solve diagnostics. residuals. UI for ranged fiducial/at-infinity
'''
mrcal-2.5/analyses/mrcal-reoptimize 0000775 0000000 0000000 00000013643 15123677724 0017533 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Loads a model, and re-runs the optimization problem used to generate it
This is useful to analyze the solve. We can generate perfect chessboard
observations, corrupted with perfect nominal noise to validate the idea that
differences observed with mrcal-show-projection-diff should be predictive by the
uncertainties reported by mrcal-show-projection-uncertainty IF the dominant
source of error is calibration-time sampling error.
By default we write the resulting model to disk into a file
INPUTMODEL-reoptimized.cameramodel. If --explore, we instead drop into a REPL.
'''
import sys
import argparse
import re
import os
def parse_args():
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--model-intrinsics',
help='''By default, all the nominal data comes from the
MODEL given in the positional argument. If
--model-intrinsics is given, the intrinsics only come
from this other model. These are applied only to the ONE
model in icam_intrinsics''')
parser.add_argument('--perfect',
action= 'store_true',
help='''Make perfect observations and add perfect noise''')
parser.add_argument('--verbose',
action = 'store_true',
help='''If given, reoptimize verbosely''')
parser.add_argument('--skip-outlier-rejection',
action='store_true',
help='''Reoptimize with no outlier rejection''')
parser.add_argument('--revive-outliers',
action='store_true',
help='''Un-mark the outliers''')
parser.add_argument('--force', '-f',
action='store_true',
default=False,
help='''By default existing models on disk are not
overwritten. Pass --force to overwrite them without
complaint''')
parser.add_argument('--outdir',
type=lambda d: d if os.path.isdir(d) else \
parser.error(f"--outdir requires an existing directory as the arg, but got '{d}'"),
help='''Directory to write the output into. If omitted,
we use the directory of the input model''')
parser.add_argument('--explore',
action = 'store_true',
help='''If given, we don't write the reoptimized model
to disk, but drop into a REPL instead ''')
parser.add_argument('model',
type=str,
help='''The input camera model. If "-", we read standard
input and write to standard output. We get the frame
poses and extrinsics from this model. If
--model-intrinsics isn't given, we get the intrinsics
from this model as well''')
args = parser.parse_args()
return args
args = parse_args()
# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README
# I import the LOCAL mrcal
sys.path[:0] = f"{os.path.dirname(os.path.realpath(__file__))}/..",
import mrcal
import numpy as np
import numpysane as nps
import time
model = mrcal.cameramodel(args.model)
if args.model == '-':
# Input read from stdin. Write output to stdout
file_output = sys.stdout
else:
if args.outdir is None:
filename_base,extension = os.path.splitext(args.model)
file_output = f"{filename_base}-reoptimized{extension}"
else:
f,extension = os.path.splitext(args.model)
directory,filename_base = os.path.split(f)
file_output = f"{args.outdir}/{filename_base}-reoptimized{extension}"
if os.path.exists(file_output) and not args.force:
print(f"ERROR: '{file_output}' already exists. Not overwriting this file. Pass -f to overwrite",
file=sys.stderr)
sys.exit(1)
optimization_inputs = model.optimization_inputs()
if args.perfect:
if args.model_intrinsics is not None:
model_intrinsics = mrcal.cameramodel(args.model_intrinsics)
if model_intrinsics.intrinsics()[0] != model.intrinsics()[0]:
print("At this time, --model-intrinsics MUST use the same lens model as the reference model",
file=sys.stderr)
sys.exit(1)
optimization_inputs['intrinsics'][model.icam_intrinsics()] = \
model_intrinsics.intrinsics()[1]
mrcal.make_perfect_observations(optimization_inputs)
######### Reoptimize
optimization_inputs['verbose'] = args.verbose
optimization_inputs['do_apply_outlier_rejection'] = not args.skip_outlier_rejection
if args.revive_outliers:
for what in ('observations_board','observations_point'):
observations = optimization_inputs[what]
print(f"Original solve has {np.count_nonzero(observations[...,2] <= 0)} outliers in {what}. Reviving them")
print("")
observations[observations[...,2] <= 0, 2] = 1.
mrcal.optimize(**optimization_inputs)
model = mrcal.cameramodel(optimization_inputs = optimization_inputs,
icam_intrinsics = model.icam_intrinsics())
if not args.explore:
note = \
"generated on {} with {}\n". \
format(time.strftime("%Y-%m-%d %H:%M:%S"),
' '.join(mrcal.shellquote(s) for s in sys.argv))
model.write(file_output, note=note)
if isinstance(file_output, str):
print(f"Wrote '{file_output}'",
file=sys.stderr)
sys.exit(0)
print("")
print("Done. The results are in the 'model' and 'optimization_inputs' variables")
print("")
import IPython
IPython.embed()
mrcal-2.5/analyses/mrcal-reproject-to-chessboard 0000775 0000000 0000000 00000016456 15123677724 0022101 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Reproject calibration images to show a chessboard-centered view
SYNOPSIS
analyses/mrcal-reproject-to-chessboard \
--image-directory images/ \
--outdir /tmp/frames-splined \
splined.cameramodel
ffmpeg \
-r 5 \
-f image2 \
-export_path_metadata 1 \
-pattern_type glob \
-i "/tmp/frames-splined/*.jpg" \
-filter:v "drawtext=text='%{metadata\\:lavf.image2dec.source_basename}':fontcolor=yellow:fontsize=48" \
-y \
/tmp/reprojected-to-chessboard.mp4
Outliers are highlighted. This is useful to visualize calibration errors. A
perfect solve would display exactly the same calibration grid with every frame.
In reality, we see the small errors in the calibration, and how they affect the
individual chessboard corner observations. The intent of this tool is to be able
to see any unmodeled chessboard deformations. It's not yet clear if this tool
can do that effectively
'''
import sys
import argparse
import re
import os
def parse_args():
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--image-path-prefix',
help='''If given, we prepend the given prefix to the image paths. Exclusive with
--image-directory''')
parser.add_argument('--image-directory',
help='''If given, we extract the filenames from the image paths in the solve, and use
the given directory to find those filenames. Exclusive
with --image-path-prefix''')
parser.add_argument('--outdir',
type=lambda d: d if os.path.isdir(d) else \
parser.error(f"--outdir requires an existing directory as the arg, but got '{d}'"),
default='.',
help='''Directory to write the output into. If omitted,
we use the current directory''')
parser.add_argument('--resolution-px-m',
default = 1000,
type = float,
help='''The resolution of the output image in pixels/m.
Defaults to 1000''')
parser.add_argument('--force', '-f',
action='store_true',
default=False,
help='''By default existing files are not overwritten. Pass --force to overwrite them
without complaint''')
parser.add_argument('model',
type=str,
help='''The input camera model. We use its
optimization_inputs''')
args = parser.parse_args()
if args.image_path_prefix is not None and \
args.image_directory is not None:
print("--image-path-prefix and --image-directory are mutually exclusive",
file=sys.stderr)
sys.exit(1)
return args
args = parse_args()
# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README
import numpy as np
import numpysane as nps
import cv2
import mrcal
try:
model = mrcal.cameramodel(args.model)
except Exception as e:
print(f"Couldn't load camera model '{args.model}': {e}", file=sys.stderr)
sys.exit(1)
optimization_inputs = model.optimization_inputs()
if optimization_inputs is None:
print("The given model MUST have the optimization_inputs for this tool to be useful",
file=sys.stderr)
sys.exit(1)
object_height_n,object_width_n = optimization_inputs['observations_board'].shape[1:3]
object_spacing = optimization_inputs['calibration_object_spacing']
# I cover a space of N+1 squares wide/tall: N-1 between all the corners + 1 on
# either side. I span squares -1..N inclusive
Nx = int(args.resolution_px_m * object_spacing * (object_width_n + 1) + 0.5)
Ny = int(args.resolution_px_m * object_spacing * (object_height_n + 1) + 0.5)
# shape (Nframes,6)
rt_ref_frame = optimization_inputs['rt_ref_frame']
rt_cam_ref = optimization_inputs['rt_cam_ref']
ifcice = optimization_inputs['indices_frame_camintrinsics_camextrinsics']
observations = optimization_inputs['observations_board']
lensmodel = optimization_inputs['lensmodel']
intrinsics_data = optimization_inputs['intrinsics']
imagepaths = optimization_inputs.get('imagepaths')
if imagepaths is None:
print("The given model MUST have the image paths for this tool to be useful",
file=sys.stderr)
sys.exit(1)
# shape (Ny,Nx,3)
p_frame = \
mrcal.ref_calibration_object(object_width_n,object_height_n,
object_spacing,
x_corner0 = -1,
x_corner1 = object_width_n,
Nx = Nx,
y_corner0 = -1,
y_corner1 = object_height_n,
Ny = Ny,
calobject_warp = optimization_inputs['calobject_warp'])
for i in range(len(ifcice)):
iframe, icamintrinsics, icamextrinsics = ifcice[i]
p_ref = \
mrcal.transform_point_rt(rt_ref_frame[iframe], p_frame)
if icamextrinsics >= 0:
p_cam = \
mrcal.transform_point_rt(rt_cam_ref[icamextrinsics], p_ref)
else:
p_cam = p_ref
# shape (Ny,Nx,2)
q = mrcal.project(p_cam, lensmodel, intrinsics_data[icamintrinsics]).astype(np.float32)
imagepath = imagepaths[i]
if args.image_path_prefix is not None:
imagepath = f"{args.image_path_prefix}/{imagepath}"
elif args.image_directory is not None:
imagepath = f"{args.image_directory}/{os.path.basename(imagepath)}"
try:
image = mrcal.load_image(imagepath,
bits_per_pixel = 24,
channels = 3)
except:
print(f"Couldn't read image at '{imagepath}'", file=sys.stderr)
sys.exit(1)
image_out = mrcal.transform_image(image,q)
# shape (Ny,Nx)
weight = observations[i,:,:,2]
mask_outlier = weight<=0
# Mark all the outliers
for iy,ix in np.argwhere(mask_outlier):
# iy,ix index corners. I need to convert the to pixels in my image
# I have pixels = linspace(-1,object_width_n,Nx)
# So:
qx = (ix+1)*(Nx-1) / (object_width_n +1)
qy = (iy+1)*(Ny-1) / (object_height_n+1)
# Red circle around the outlier
red = (0,0,255)
cv2.circle(image_out,
center = np.round(np.array((qx,qy))).astype(int),
radius = 10,
color = red,
thickness = 2)
imagepath_out = f"{args.outdir}/{os.path.basename(imagepath)}"
if not args.force and os.path.exists(imagepath_out):
print("File already exists: '{imagepath_out}'. Not overwriting; pass --force to overwrite. Giving up.",
file=sys.stderr)
sys.exit(1)
mrcal.save_image(imagepath_out, image_out)
print(f"Wrote '{imagepath_out}'")
mrcal-2.5/analyses/noncentral/ 0000775 0000000 0000000 00000000000 15123677724 0016456 5 ustar 00root root 0000000 0000000 mrcal-2.5/analyses/noncentral/trace-noncentral-ray.py 0000775 0000000 0000000 00000004304 15123677724 0023064 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Visualize noncentrality of a model
Given a model and query pixel this function
- Computes a ray from the camera center to the unprojection at infinity
- Samples different distances along this ray
- Projects them, and visualizes the difference projection from the sample point
As we get closer to the camera, stronger and stronger noncentral effects are
observed
'''
import sys
import numpy as np
import numpysane as nps
import gnuplotlib as gp
import re
import mrcal
model_filename = sys.argv[1]
qref = np.array((100,100), dtype=float)
model = mrcal.cameramodel(model_filename)
lensmodel,intrinsics_data = model.intrinsics()
if not mrcal.lensmodel_metadata_and_config(lensmodel)['noncentral']:
print("The given model isn't noncentral. Nothing to do", file=sys.stderr)
sys.exit(1)
if not re.match('^LENSMODEL_CAHVORE_', lensmodel):
print("This is only implemented for CAHVORE today", file=sys.stderr)
sys.exit(1)
# Special-case to centralize CAHVORE
intrinsics_data_centralized = intrinsics_data.copy()
intrinsics_data_centralized[-3:] = 0
v_at_infinity = \
mrcal.unproject(qref,
lensmodel, intrinsics_data_centralized,
normalize = True)
if 0:
# another thing to do is to compute deltaz
# dz is a function of th
# pc = p - [0,0,dz]
# tan(th) = pxy / (pz - dz)
# q = project_central(pc)
# At infinity pz >> dz -> tan(th) = pxy/pz
# At p I have q = project_central(pc)
# unproject_central(q) = k*pc
# p - k*pc = [0,0,dz]
d = 0.2
p = v_at_infinity*d
q = mrcal.project (p, lensmodel, intrinsics_data)
vc = mrcal.unproject(q, lensmodel, intrinsics_data_centralized, normalize = True)
k01 = p[:2]/vc[:2]
dz = p[2] - k01*vc[2]
import IPython
IPython.embed()
sys.exit()
Ndistances = 100
d = np.linspace(0.01, 10., Ndistances)
# shape (Ndistances, 3)
p = nps.dummy(d, -1) * v_at_infinity
# shape (Ndistances, 2)
q = mrcal.project(p, *model.intrinsics())
# shape (Ndistances,)
qshift = nps.mag(q - qref)
gp.plot( d, qshift,
_with = 'linespoints',
xlabel = 'Distance (m)',
ylabel = 'Pixel shift (pixels)',
wait = True )
mrcal-2.5/analyses/outlierness/ 0000775 0000000 0000000 00000000000 15123677724 0016667 5 ustar 00root root 0000000 0000000 mrcal-2.5/analyses/outlierness/outlierness-test.py 0000775 0000000 0000000 00000044333 15123677724 0022604 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
'''This is a set of simple experiments to test the outlier-rejection and
sensitivity logic'''
import sys
import os
import numpy as np
import numpysane as nps
import gnuplotlib as gp
usage = "Usage: {} order Npoints noise_stdev".format(sys.argv[0])
if len(sys.argv) != 4:
print usage
sys.exit(1)
try:
order = int( sys.argv[1]) # order. >= 1
N = int( sys.argv[2]) # This many points in the dataset
noise_stdev = float(sys.argv[3]) # The dataset is corrupted thusly
except:
print usage
sys.exit(1)
if order < 1 or N <= 0 or noise_stdev <= 0:
print usage
sys.exit(1)
Nquery = 70 # This many points for post-fit uncertainty-evaluation
reference_equation = '0.1*(x+0.2)**2. + 3.0'
def report_mismatch_relerr(a,b, what):
relerr = np.abs(a-b) / ( (a+b)/2.)
if relerr > 1e-6:
print "MISMATCH for {}: relerr = {}, a = {}, b = {},".format(what,relerr,a,b);
def report_mismatch_abserr(a,b, what):
abserr = np.abs(a-b)
if abserr > 1e-6:
print "MISMATCH for {}: abserr = {}, a = {}, b = {},".format(what,abserr,a,b);
def model_matrix(q, order):
r'''Returns the model matrix S for particular domain points
Here the "order" is the number of parameters in the fit. Thus order==2 means
"linear" and order==3 means "quadratic""
'''
q = nps.atleast_dims(q,-1)
return nps.transpose(nps.cat(*[q ** i for i in range(order)]))
def func_reference(q):
'''Reference function: reference_equation
Let's say I care about the range [0..1]. It gets less linear as I get
further away from 0
'''
# needs to manually match 'reference_equation' above
return 0.1 * (q+0.2)*(q+0.2) + 3.0
def func_hypothesis(q, b):
'''Hypothesis based on parameters
'''
S = model_matrix(q, len(b))
return nps.matmult(b, nps.transpose(S))
def compute_outliernesses(J, x, jq, k_dima, k_cook):
'''Computes all the outlierness/Cook's D metrics
I have 8 things I can compute coming from 3 yes/no choices. These are all
very similar, with two pairs actually coming out identical. I choose:
- Are we detecting outliers, or looking at effects of a new query point?
- Dima's outlierness factor or Cook's D
- Look ONLY at the effect on the other variables, or on the other variables
AND self?
If we're detecting outliers, we REMOVE measurements from the dataset, and
see what happens to the fit. If we're looking at effects of a new query
point, we see what happend if we ADD measurements
Dima's outlierness factor metric looks at what happens to the cost function
E = norm2(x). Specifically I look at
(norm2(x_before) - norm(x_after))/Nmeasurements
Cook's D instead looks at
(norm2(x_before - x_after)) * k
for some constant k.
Finally, we can decide whether to include the effects on the measurements
we're adding/removing, or not.
Note that here I only look at adding/removing SCALAR measurements
=============
This is similar-to, but not exactly-the-same-as Cook's D. I assume the least
squares fit optimizes a cost function E = norm2(x). The outlierness factor I
return is
f = 1/Nmeasurements (E(outliers and inliers) - E(inliers only))
For a scalar measurement, this solves to
k = xo^2 / Nmeasurements
B = 1.0/(jt inv(JtJ) j - 1)
f = -k * B
(see the comment in dogleg_getOutliernessFactors() for a description)
Note that my metric is proportional to norm2(x_io) - norm2(x_i). This is NOT
the same as Cook's distance, which is proportional to norm2(x_io - x_i).
It's not yet obvious to me which is better
There're several slightly-different definitions of Cook's D and of a
rule-of-thumb threshold floating around on the internet. Wikipedia says:
D = norm2(x_io - x_i)^2 / (Nstate * norm2(x_io)/(Nmeasurements - Nstate))
D_threshold = 1
An article https://www.nature.com/articles/nmeth.3812 says
D = norm2(x_io - x_i)^2 / ((Nstate+1) * norm2(x_io)/(Nmeasurements - Nstate -1))
D_threshold = 4/Nmeasurements
Here I use the second definition. That definition expands to
k = xo^2 / ((Nstate+1) * norm2(x_io)/(Nmeasurements - Nstate -1))
B = 1.0/(jt inv(JtJ) j - 1)
f = k * (B + B*B)
'''
Nmeasurements,Nstate = J.shape
# The A values for each measurement
Aoutliers = nps.inner(J, nps.transpose(np.linalg.pinv(J)))
Aquery = nps.inner(jq, nps.transpose(np.linalg.solve(nps.matmult(nps.transpose(J),J), nps.transpose(jq))))
def dima():
k = k_dima
k = 1
# Here the metrics are linear, so self + others = self_others
def outliers():
B = 1.0 / (Aoutliers - 1.0)
return dict( self = k * x*x,
others = k * x*x*(-B-1),
self_others = k * x*x*(-B ))
def query():
B = 1.0 / (Aquery + 1.0)
return dict( self = k * ( B*B),
others = k * (B-B*B),
self_others = k * (B))
return dict(outliers = outliers(),
query = query())
def cook():
k = k_cook
k = 1
# Here the metrics maybe aren't linear (I need to think about it), so
# maybe self + others != self_others. I thus am not returning the "self"
# metric
def outliers():
B = 1.0 / (Aoutliers - 1.0)
return dict( self_others = k * x*x*(B+B*B ) ,
others = k * x*x*(-B-1))
def query():
B = 1.0 / (Aquery + 1.0)
return dict( self_others = k * (1-B) ,
others = k * (B-B*B))
return dict(outliers = outliers(),
query = query())
return dict(cook = cook(),
dima = dima())
def outlierness_test(J, x, f, outlierness, k_dima, k_cook, i=0):
r'''Test the computation of outlierness
I have an analytical expression for this computed in
compute_outliernesses(). This explicitly computes the quantity represented
by compute_outliernesses() to make sure that that analytical expression is
correct
'''
# I reoptimize without measurement i
E0 = nps.inner(x,x)
J1 = nps.glue(J[:i,:], J[(i+1):,:], axis=-2)
f1 = nps.glue(f[:i ], f[(i+1): ], axis=-1)
b1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1)))
x1 = nps.matmult(b1, nps.transpose(J1)) - f1
E1 = nps.inner(x1,x1)
report_mismatch_relerr( (E0-E1) * k_dima,
outlierness['self_others'][i],
"self_others outlierness computed analytically, explicitly")
report_mismatch_relerr( (E0-x[i]*x[i] - E1) * k_dima,
outlierness['others'][i],
"others outlierness computed analytically, explicitly")
def CooksD_test(J, x, f, CooksD, k_dima, k_cook, i=0):
r'''Test the computation of Cook's D
I have an analytical expression for this computed in
compute_outliernesses(). This explicitly computes the quantity represented
by compute_outliernesses() to make sure that that analytical expression is
correct
'''
# I reoptimize without measurement i
Nmeasurements,Nstate = J.shape
J1 = nps.glue(J[:i,:], J[(i+1):,:], axis=-2)
f1 = nps.glue(f[:i ], f[(i+1): ], axis=-1)
b1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1)))
x1 = nps.matmult(b1, nps.transpose(J)) - f
dx = x1-x
report_mismatch_relerr( nps.inner(dx,dx) * k_cook,
CooksD['self_others'][i],
"self_others CooksD computed analytically, explicitly")
report_mismatch_relerr( (nps.inner(dx,dx) - dx[i]*dx[i]) * k_cook,
CooksD['others'][i],
"others CooksD computed analytically, explicitly")
def outlierness_query_test(J,b,x, f, query,fquery_ref, outlierness_nox, k_dima, k_cook, i=0):
r'''Test the concept of outlierness for querying hypothetical data
fquery_test = f(q) isn't true here. If it WERE true, the x of the query
point would be 0 (we fit the model exactly), so the outlierness factor would
be 0 also
'''
# current solve
E0 = nps.inner(x,x)
query = query [i]
fquery_ref = fquery_ref[i]
# I add a new point, and reoptimize
fquery = func_hypothesis(query,b)
xquery = fquery - fquery_ref
jquery = model_matrix(query, len(b))
J1 = nps.glue(J, jquery, axis=-2)
f1 = nps.glue(f, fquery_ref, axis=-1)
b1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1)))
x1 = nps.matmult(b1, nps.transpose(J1)) - f1
E1 = nps.inner(x1,x1)
report_mismatch_relerr( (x1[-1]*x1[-1]) * k_dima,
outlierness_nox['self'][i]*xquery*xquery,
"self query-outlierness computed analytically, explicitly")
report_mismatch_relerr( (E1-x1[-1]*x1[-1] - E0) * k_dima,
outlierness_nox['others'][i]*xquery*xquery,
"others query-outlierness computed analytically, explicitly")
report_mismatch_relerr( (E1 - E0) * k_dima,
outlierness_nox['self_others'][i]*xquery*xquery,
"self_others query-outlierness computed analytically, explicitly")
def CooksD_query_test(J,b,x, f, query,fquery_ref, CooksD_nox, k_dima, k_cook, i=0):
r'''Test the concept of CooksD for querying hypothetical data
fquery_test = f(q) isn't true here. If it WERE true, the x of the query
point would be 0 (we fit the model exactly), so the outlierness factor would
be 0 also
'''
# current solve
Nmeasurements,Nstate = J.shape
query = query [i]
fquery_ref = fquery_ref[i]
# I add a new point, and reoptimize
fquery = func_hypothesis(query,b)
xquery = fquery - fquery_ref
jquery = model_matrix(query, len(b))
J1 = nps.glue(J, jquery, axis=-2)
f1 = nps.glue(f, fquery_ref, axis=-1)
b1 = nps.matmult( f1, nps.transpose(np.linalg.pinv(J1)))
x1 = nps.matmult(b1, nps.transpose(J1)) - f1
dx = x1[:-1] - x
dx_both = x1 - nps.glue(x,xquery, axis=-1)
report_mismatch_relerr( nps.inner(dx_both,dx_both)*k_cook,
CooksD_nox['self_others'][i]*xquery*xquery,
"self_others query-CooksD computed analytically, explicitly")
report_mismatch_relerr( nps.inner(dx,dx)*k_cook,
CooksD_nox['others'][i]*xquery*xquery,
"others query-CooksD computed analytically, explicitly")
def Var_df(J, squery, stdev):
r'''Propagates noise in input to noise in f
noise in input -> noise in params -> noise in f
db ~ M dm where M = inv(JtJ)Jt
df = df/db db
df/db = squery
Var(dm) = stdev^2 I ->
Var(df) = stdev^2 squery inv(JtJ) Jt J inv(JtJ) squeryt =
= stdev^2 squery inv(JtJ) squeryt
This function broadcasts over squery
'''
return \
nps.inner(squery,
nps.transpose(np.linalg.solve(nps.matmult(nps.transpose(J),J),
nps.transpose(squery)))) *stdev*stdev
def histogram(x, **kwargs):
h,edges = np.histogram(x, bins=20, **kwargs)
centers = (edges[1:] + edges[0:-1])/2
return h,centers,edges[1]-edges[0]
def generate_dataset(N, noise_stdev):
q = np.random.rand(N)
fref = func_reference(q)
fnoise = np.random.randn(N) * noise_stdev
f = fref + fnoise
return q,f
def fit(q, f, order):
S = model_matrix(q, order)
J = S
b = nps.matmult( f, nps.transpose(np.linalg.pinv(S)))
x = func_hypothesis(q,b) - f
return b,J,x
def test_order(q,f, query, order):
# I look at linear and quadratic models: a0 + a1 q + a2 q^2, with a2=0 for the
# linear case. I use plain least squares. The parameter vector is [a0 a1 a2]t. S
# = [1 q q^2], so the measurement vector x = S b - f. E = norm2(x). J = dx/db =
# S.
#
# Note the problem "order" is the number of parameters, so a linear model has
# order==2
b,J,x = fit(q,f,order)
Nmeasurements,Nstate = J.shape
k_dima = 1.0/Nmeasurements
k_cook = 1.0/((Nstate + 1.0) * nps.inner(x,x)/(Nmeasurements - Nstate - 1.0))
report_mismatch_abserr(np.linalg.norm(nps.matmult(x,J)), 0, "Jtx")
squery = model_matrix(query, order)
fquery = func_hypothesis(query, b)
metrics = compute_outliernesses(J,x, squery, k_dima, k_cook)
outlierness_test(J, x, f, metrics['dima']['outliers'], k_dima, k_cook, i=10)
CooksD_test (J, x, f, metrics['cook']['outliers'], k_dima, k_cook, i=10)
outlierness_query_test(J,b,x,f, query, fquery + 1.2e-3, metrics['dima']['query'], k_dima, k_cook, i=10 )
CooksD_query_test (J,b,x,f, query, fquery + 1.2e-3, metrics['cook']['query'], k_dima, k_cook, i=10 )
Vquery = Var_df(J, squery, noise_stdev)
return \
dict( b = b,
J = J,
x = x,
Vquery = Vquery,
squery = squery,
fquery = fquery,
metrics = metrics,
k_dima = k_dima,
k_cook = k_cook )
q,f = generate_dataset(N, noise_stdev)
query = np.linspace(-1,2, Nquery)
stats = test_order(q,f, query, order)
def show_outlierness(order, N, q, f, query, cooks_threshold, **stats):
p = gp.gnuplotlib(equation='1.0 title "Threshold"',
title = "Outlierness with order={} Npoints={} stdev={}".format(order, N, noise_stdev))
p.plot( (q, stats['metrics']['dima']['outliers']['self_others']/stats['dimas_threshold'],
dict(legend="Dima's self+others outlierness / threshold",
_with='points')),
(q, stats['metrics']['dima']['outliers']['others']/stats['dimas_threshold'],
dict(legend="Dima's others-ONLY outlierness / threshold",
_with='points')),
(q, stats['metrics']['cook']['outliers']['self_others']/cooks_threshold,
dict(legend="Cook's self+others outlierness / threshold",
_with='points')),
(q, stats['metrics']['cook']['outliers']['others']/cooks_threshold,
dict(legend="Cook's others-ONLY outlierness / threshold",
_with='points')))
return p
def show_uncertainty(order, N, q, f, query, cooks_threshold, **stats):
coeffs = stats['b']
fitted_equation = '+'.join(['{} * x**{}'.format(coeffs[i], i) for i in range(len(coeffs))])
p = gp.gnuplotlib(equation='({})-({}) title "Fit error off ground truth; y2 axis +- noise stdev" axis x1y2'.format(reference_equation,fitted_equation),
title = "Uncertainty with order={} Npoints={} stdev={}".format(order, N, noise_stdev),
ymin=0,
y2range = (-noise_stdev, noise_stdev),
_set = 'y2tics'
)
# p.plot(
# (query, np.sqrt(stats['Vquery]),
# dict(legend='expectederr (y2)', _with='lines', y2=1)),
# (query, stats['metrics']['dima']['query']['self_others']*noise_stdev*noise_stdev / stats['dimas_threshold'],
# dict(legend="Dima's self+others query / threshold",
# _with='linespoints')),
# (query, stats['metrics']['dima']['query']['others']*noise_stdev*noise_stdev / stats['dimas_threshold'],
# dict(legend="Dima's others-ONLY query / threshold",
# _with='linespoints')),
# (query, stats['metrics']['cook']['query']['self_others']*noise_stdev*noise_stdev / cooks_threshold,
# dict(legend="Cook's self+others query / threshold",
# _with='linespoints')),
# (query, stats['metrics']['cook']['query']['others']*noise_stdev*noise_stdev / cooks_threshold,
# dict(legend="Cook's others-ONLY query / threshold",
# _with='linespoints')))
p.plot(
# (query, np.sqrt(Vquery),
# dict(legend='Expected error due to input noise (y2)', _with='lines', y2=1)),
(query, np.sqrt(stats['metrics']['dima']['query']['self'])*noise_stdev,
dict(legend="Dima's self-ONLY; 1 point",
_with='linespoints')),
(query, np.sqrt(stats['metrics']['dima']['query']['others'])*noise_stdev,
dict(legend="Dima's others-ONLY ALL {} points".format(Nquery),
_with='linespoints')),
# (query, np.sqrt(stats['metrics']['dima']['query']['others'])*noise_stdev * Nquery,
# dict(legend="Dima's others-ONLY 1 point average",
# _with='linespoints')),
)
return p
def show_fit (order, N, q, f, query, cooks_threshold, **stats):
p = gp.gnuplotlib(equation='{} with lines title "reference"'.format(reference_equation),
xrange=[-1,2],
title = "Fit with order={} Npoints={} stdev={}".format(order, N, noise_stdev))
p.plot((q, f, dict(legend = 'input data', _with='points')),
(query, stats['fquery'] + np.sqrt(stats['Vquery'])*np.array(((1,),(0,),(-1,),)), dict(legend = 'stdev_f', _with='lines')))
return p
def show_distribution(outlierness):
h,c,w = histogram(outlierness)
raise Exception("not done")
#gp.plot()
# This is hoaky, but reasonable, I think. Some of the unscaled metrics are
# identical between mine and Cook's expressions. So I scale Cook's 4/N threshold
# to apply to me. Works ok.
cooks_threshold = 4.0 / N
stats['dimas_threshold'] = cooks_threshold / stats['k_cook'] * stats['k_dima']
# These all show interesting things; turn one of them on
plots = [ show_outlierness (order, N, q, f, query, cooks_threshold, **stats),
show_fit (order, N, q, f, query, cooks_threshold, **stats),
show_uncertainty (order, N, q, f, query, cooks_threshold, **stats)
]
for p in plots:
if os.fork() == 0:
p.wait()
sys.exit()
os.wait()
# Conclusions:
#
# - Cook's 4/N threshold looks reasonable.
#
# - For detecting outliers my self+others metric is way too outlier-happy. It
# makes most of the linear fit points look outliery
#
# - For uncertainty, Dima's self+others is the only query metric that's maybe
# usable. It qualitatively a negated and shifted Cook's self+others
mrcal-2.5/analyses/sfm-experiments/ 0000775 0000000 0000000 00000000000 15123677724 0017441 5 ustar 00root root 0000000 0000000 mrcal-2.5/analyses/sfm-experiments/sfm.py 0000775 0000000 0000000 00000200763 15123677724 0020613 0 ustar 00root root 0000000 0000000 #!/usr/bin/python3
r'''
'''
import sys
import numpy as np
import numpysane as nps
import gnuplotlib as gp
import glob
import re
import os
import sqlite3
import pyopengv
import scipy.linalg
# Need bleeding-edge mrcal. Using 2.5~6-1 right now
import mrcal
np.set_printoptions(linewidth = 800000)
def imread(filename, decimation):
image = mrcal.load_image(filename, bits_per_pixel = 8, channels = 1)
return image, image[::decimation, ::decimation]
def plot_flow(image, flow, decimation,
**kwargs):
H,W = flow.shape[:2]
# each has shape (H,W)
xx,yy = np.meshgrid(np.arange(0,W,decimation),
np.arange(0,H,decimation))
# shape (H,W,4)
vectors = \
nps.glue( nps.dummy(xx,-1),
nps.dummy(yy,-1),
flow[::decimation, ::decimation, :],
axis = -1);
vectors = nps.clump(vectors, n=2)
gp.plot( (image, dict(_with='image',
tuplesize = 3)),
(vectors,
dict(_with='vectors lc "red"',
tuplesize=-4)),
_set = 'palette gray',
unset = 'colorbox',
square = True,
_xrange = (0,W),
_yrange = (H,0),
**kwargs)
def match_looks_valid(q, match, flow):
flow_observed = q[1] - q[0]
flow_err_sq = nps.norm2(flow_observed - flow)
return \
match.distance < 30 and \
flow_err_sq < 2*2
def decompose_essential_matrix(E):
'''Returns an R,t defined by an essential matrix
E = R * skew_symmetric(t) = R * T
I know that cross(t,t) = T * t = 0, so I can get t as an eigenvector of E
corresponding to an eigenvalue of 0. If I have T then I need to solve E = R * T.
This is the procrustes problem that I can solve with
mrcal.align_procrustes_vectors_R01()
'''
l,t = np.linalg.eig(E)
i = np.argmin(np.abs(l))
if np.abs(l[i]) > 1e-10:
raise Exception("E doesn't have a 0 eigenvalue")
if nps.norm2(t[:,i].imag) > 1e-10:
raise Exception("null eigenvector of E has non-0 imaginary components")
t = t[:,i].real
# The "true" t is k*t for some unknown constant k
# And the "true" T is k*T for some unknown constant k
T = mrcal.skew_symmetric(t)
# E = k*R*T -> EtE = k^2 TtT
ksq = nps.matmult(nps.transpose(E),E) / nps.matmult(nps.transpose(T),T)
mean_ksq = np.mean(ksq)
if nps.norm2( ksq.ravel() - mean_ksq ) > 1e-10:
raise Exception("t doesn't have a consistent scale")
k = np.sqrt(mean_ksq)
t *= k
T *= k
# I now have t and T with the right scale, BUT STILL WITH AN UNKNOWN SIGN. I
# report both versions
Rt = np.empty((2,4,3), dtype=float)
Rt[0,:3,:] = \
mrcal.align_procrustes_vectors_R01(nps.transpose(E),
nps.transpose(T))
Rt[0,3,:] = t
if nps.norm2((nps.matmult(Rt[0,:3,:],T) - E).ravel()) > 1e-10:
raise Exception("Non-fitting rotation")
t *= -1.
T *= -1.
Rt[1,:3,:] = \
mrcal.align_procrustes_vectors_R01(nps.transpose(E),
nps.transpose(T))
Rt[1,3,:] = t
if nps.norm2((nps.matmult(Rt[1,:3,:],T) - E).ravel()) > 1e-10:
raise Exception("Non-fitting rotation")
return Rt
def seed_rt10_pair_from_far_subset(q0, q1, mask_far):
r'''Estimates a transform between two cameras
This method ingests two sets of corresponding features, with a subset of these
features deemed to be "far". It then
- Computes a Procrustes fit on the "far" features to get an estimate for the
rotation. This is valid because observations at infinity are not affected by
the relatively tiny translations, and I only need to rotate the vectors
properly.
- Assumes this rotation is correct, and uses all the features to estimate the
translation. This part is more involved, so I write it up here
I use the geometric triangulation expression derived here:
https://github.com/dkogan/mrcal/blob/8be76fc28278f8396c0d3b07dcaada2928f1aae0/triangulation.cc#L112
I assume that I'm triangulating normalized v0,v1 both expressed in cam-0
coordinates. And I have a t01 translation that I call "t" from here on. This is
unique only up-to-scale, so I assume that norm2(t) = 1. The geometric
triangulation from the above link says that
[k0] = 1/(v0.v0 v1.v1 -(v0.v1)**2) [ v1.v1 v0.v1][ t01.v0]
[k1] [ v0.v1 v0.v0][-t01.v1]
The midpoint p is
p = (k0 v0 + t01 + k1 v1)/2
I assume that v are normalized and I represent k as a vector. I also define
c = inner(v0,v1)
So
k = 1/(1 - c^2) [1 c] [ v0t] t
[c 1] [-v1t]
I define
A = 1/(1 - c^2) [1 c] This is a 2x2 array
[c 1]
B = [ v0t] This is a 2x3 array
[-v1t]
V = [v0 v1] This is a 3x2 array
Note that none of A,B,V depend on t.
So
k = A B t
Then
p = (k0 v0 + t01 + k1 v1)/2
= (V k + t)/2
= (V A B t + t)/2
= (I + V A B) t/2
Each triangulated error is
e = mag(p - k0 v0)
I split A into its rows
A = [ a0t ]
[ a1t ]
Then
e = p - k0 v0
= (I + V A B) t/2 - v0 a0t B t
= I t/2 + V A B t/2 - v0 a0t B t
= I t/2 + v0 a0t B t/2 + v1 a1t B t/2 - v0 a0t B t
= I t/2 - v0 a0t B t/2 + v1 a1t B t/2
= ((I - v0 a0t B + v1 a1t B) t) / 2
= ((I + (- v0 a0t + v1 a1t) B) t) / 2
= ((I - Bt A B) t) / 2
I define a joint error function I'm optimizing as the sum of all the individual
triangulation errors:
E = sum(norm2(e_i))
Each component is
norm2(e) = 1/4 tt (I - Bt A B)t (I - Bt A B) t
= 1/4 tt (I - 2 Bt A B + Bt A B Bt A B ) t
B Bt = [1 -c]
[-c 1]
B Bt A = 1/(1 - c^2) [1 -c] [1 c]
[-c 1] [c 1]
= 1/(1 - c^2) [1-c^2 0 ]
[0 1-c^2 ]
= I
-> norm2(e) = 1/4 tt (I - 2 Bt A B + Bt A B) t
= 1/4 tt (I - Bt A B) t
= 1/4 - 1/4 tt Bt A B t
So
E = N/4 - 1/4 tt sum(Bt A B) t
I let
M = sum(Bt A B)
M = sum(Mi)
Mi = Bt A B
So
E = N/4 - 1/4 tt M t
= N/4 - 1/4 lambda
So to minimize E I find t that is the eigenvector of M that corresponds to its
largest eigenvalue lambda. Furthermore, lambda depends on the rotation. If I
couldn't estimate the rotation from far-away features I can solve the
eigenvalue-optimization problem to maximize lambda.
More simplification:
Mi = Bt A B = [ v0 -v1 ] A [ v0t]
[-v1t]
= 1/(1-c^2) [ v0 - v1 c v0 c - v1] [ v0t]
[-v1t]
= 1/(1-c^2) ((v0 - v1 c) v0t - (v0 c - v1) v1t)
c = v0t v1 ->
F0 = v0 v0t
F1 = v1 v1t
-> Mi = 1/(1-c^2) (v0 v0t - v1 v1t v0 v0t + v1 v1t - v0 v0t v1 v1t)
= 1/(1-c^2) (F0 + F1 - (F1 F0 + F0 F1))
= (F0 - F1)^2 / (1 - c^2)
= ((F0 - F1)/s)^2
where s = mag(cross(v0,v1))
tt M t = sum( norm2((F0i - F1i)/si t) )
Let Di = (F0i - F1i)/si
I want to maximize sum( norm2(Di t) )
(F0 - F1)/s = (v0 v0t - v1 v1t) / mag(cross(v0,v1))
~ (v0 v0t - R v1 v1t Rt) / mag(cross(v0,R v1))
experiments:
= 1/(1-c^2) (F0 + F1 - (F1 F0 + F0 F1))
= 1/(1-c^2) (v0 v0t + v1 v1t - (c v0 v1t + c v1 v0t))
'''
# shape (N,3)
# These are in their LOCAL coord system
v0 = mrcal.unproject(q0, *model.intrinsics(),
normalize = True)
v1 = mrcal.unproject(q1, *model.intrinsics(),
normalize = True)
R01 = mrcal.align_procrustes_vectors_R01(v0[mask_far], v1[mask_far])
# can try to do outlier rejection here:
# co = nps.inner(v0[mask_far], mrcal.rotate_point_R(R01, v1[mask_far]))
# gp.plot(co)
# Keep all all non-far points initially
mask_keep_near = ~mask_far
# I only use the near features to compute t01. The far features don't affect
# t very much, and they'll have c ~ 1 and A ~ infinity
# shape (N,3)
v0_cam0coords = v0
v1_cam0coords = mrcal.rotate_point_R(R01, v1)
# shape (N,)
c = nps.inner(v0_cam0coords[mask_keep_near],
v1_cam0coords[mask_keep_near])
# Any near features that have parallel rays is disqualified
mask_parallel = np.abs(1. - c) < 1e-6
mask_keep_near[np.nonzero(mask_keep_near)[0][mask_parallel]] = False
# Can try to do outlier rejection here. At t=0 all points should be
# convergent or all should be divergent. Any non-consensus points are
# outliers
# p = mrcal.triangulate_geometric(v0[~mask_far],
# mrcal.rotate_point_R(R01, v1[~mask_far]),
# np.zeros((3,)))
# mask_divergent = nps.norm2(p) == 0
def compute_t(v0_cam0coords, v1_cam0coords):
# shape (N,)
c = nps.inner(v0_cam0coords,
v1_cam0coords)
N = len(c)
# shape (N,2,2)
A = np.ones((N,2,2), dtype=float)
A[:,0,1] = c
A[:,1,0] = c
A /= nps.mv(1. - c*c, -1,-3)
# shape (N,2,3)
B = np.empty((N,2,3), dtype=float)
B[:,0,:] = v0_cam0coords
B[:,1,:] = -v1_cam0coords
# shape (3,3)
M = np.sum( nps.matmult(nps.transpose(B), A, B), axis = 0 )
l,v = mrcal.sorted_eig(M)
# The answer is the eigenvector corresponding to the biggest eigenvalue
t01 = v[:,2]
# Almost done. I want either t or -t. The wrong one will produce
# mostly triangulations behind me
k = nps.matmult(A,B, nps.transpose(t01))[..., 0]
mask_divergent_t = (k[:,0] <= 0) + (k[:,1] <= 0)
mask_divergent_negt = (k[:,0] >= 0) + (k[:,1] >= 0)
N_divergent_t = np.count_nonzero( mask_divergent_t )
N_divergent_negt = np.count_nonzero( mask_divergent_negt )
if N_divergent_t == 0 or N_divergent_negt == 0:
# from before: norm2(e) = 1/4 - 1/4 tt Bt A B t
# shape (N,2,1)
Bt = nps.dummy(nps.inner(B, t01),
-1)
# shape (N,1,1)
tBtABt = nps.matmult(nps.transpose(Bt), A, Bt)
# shape (N,)
tBtABt = tBtABt[:,0,0]
norm2e = 1/4 * (1 - tBtABt)
else:
norm2e = None
if N_divergent_t < N_divergent_negt:
return t01, mask_divergent_t, N_divergent_t, norm2e
else:
return -t01, mask_divergent_negt, N_divergent_negt, norm2e
i_iteration = 0
while True:
print(f"seed_rt10_pair_from_far_subset() iteration {i_iteration}")
t01, mask_divergent, Ndivergent, norm2e = \
compute_t(v0_cam0coords[mask_keep_near],
v1_cam0coords[mask_keep_near])
if Ndivergent == 0:
# No divergences, and we have norm2e available. I look through
# norm2e, and throw away outliers there. If there aren't any of
# those either, I'm done.
mask_convergent_outlier = norm2e > 0.04
if not np.any(mask_convergent_outlier):
# no outliers. I'm done!
break
print(f"No divergences, but have {np.count_nonzero(mask_convergent_outlier)} outliers")
mask_outlier = mask_convergent_outlier
else:
# I have divergences. Mark these as outliers, and move on
print(f"saw {Ndivergent} divergences. Total len(v) = {len(v0)}")
mask_outlier = mask_divergent
mask_keep_near[np.nonzero(mask_keep_near)[0][mask_outlier]] = False
i_iteration += 1
Rt01 = nps.glue(R01, t01, axis=-2)
Rt10 = mrcal.invert_Rt(Rt01)
rt10 = mrcal.rt_from_Rt(Rt10)
return \
rt10, \
(mask_keep_near + mask_far)
def pairwise_solve_kneip(v0, v1):
Rt01 = np.empty((4,3), dtype=float)
Rt01[:3,:] = pyopengv.relative_pose_eigensolver(v0, v1,
# seed
mrcal.identity_R())
# opengv should do this too, but its Python bindings are lacking. I
# recompute the t myself for now
# shape (N,3)
c = np.cross(v0, mrcal.rotate_point_R(Rt01[:3,:], v1))
l,v = mrcal.sorted_eig(np.sum(nps.outer(c,c), axis=0))
# t is the eigenvector corresponding to the smallest eigenvalue
Rt01[3,:] = v[:,0]
# Almost done. I want either t or -t. The wrong one will produce
# mostly triangulations behind me
p_t = mrcal.triangulate_geometric(v0, v1,
v_are_local = True,
Rt01 = Rt01 )
mask_divergent_t = (nps.norm2(p_t) == 0)
N_divergent_t = np.count_nonzero( mask_divergent_t )
Rt01_negt = Rt01 * nps.transpose(np.array((1,1,1,-1),))
p_negt = mrcal.triangulate_leecivera_mid2(v0, v1,
v_are_local = True,
Rt01 = Rt01_negt )
mask_divergent_negt = (nps.norm2(p_negt) == 0)
N_divergent_negt = np.count_nonzero( mask_divergent_negt )
if N_divergent_t != 0 and N_divergent_negt != 0:
# We definitely have divergences. Mark them as outliers, and move on
if N_divergent_t < N_divergent_negt: return Rt01, mask_divergent_t, N_divergent_t
else: return Rt01_negt, mask_divergent_negt, N_divergent_negt
# Nothing is divergent. I look for outliers
if N_divergent_t == 0:
p = p_t
mask_divergent = mask_divergent_t
N_divergent = N_divergent_t
else:
p = p_negt
mask_divergent = mask_divergent_negt
N_divergent = N_divergent_negt
Rt01 = Rt01_negt
costh = nps.inner(p, v0) / nps.mag(p)
costh_threshold = np.cos(1.0 * np.pi/180.)
mask_convergent_outliers = costh < costh_threshold
if not np.any(mask_convergent_outliers):
# no outliers. I'm done!
return Rt01, mask_divergent, N_divergent
Nmask_convergent_outliers = np.count_nonzero(mask_convergent_outliers)
print(f"No divergences, but have {Nmask_convergent_outliers} outliers")
return Rt01, mask_convergent_outliers, Nmask_convergent_outliers
def seed_rt10_pair_kneip_eigensolver(q0, q1):
r'''Estimates a transform between two cameras
opengv does all the work
'''
# shape (N,3)
# These are in their LOCAL coord system
v0 = mrcal.unproject(q0, *model.intrinsics(),
normalize = True)
v1 = mrcal.unproject(q1, *model.intrinsics(),
normalize = True)
# Keep all all non-far points initially
mask_inliers = np.ones( (q0.shape[0],), dtype=bool )
i_iteration = 0
while True:
print(f"seed_rt10_pair_kneip_eigensolver() iteration {i_iteration}")
Rt01, mask_outlier, Noutliers = pairwise_solve_kneip(v0[mask_inliers], v1[mask_inliers])
print(f"saw {Noutliers} outliers. Total len(v) = {len(v0)}")
if Noutliers == 0:
break
mask_inliers[np.nonzero(mask_inliers)[0][mask_outlier]] = False
i_iteration += 1
Rt10 = mrcal.invert_Rt(Rt01)
rt10 = mrcal.rt_from_Rt(Rt10)
return \
rt10, \
mask_inliers
def seed_rt10_pair(i0, q0, q1):
if False:
# driving forward
return np.array((-0.0001,0.0001,0.00001, 0.2,0.2,-.95), dtype=float), \
None
elif True:
# My method using known-far features
rt10, mask_inliers = \
seed_rt10_pair_from_far_subset(q0,q1,
(q0[:,1] < 800) * (q1[:,1] < 800))
return rt10, mask_inliers
elif True:
# The method in opengv. Similar to my method, but better!
rt10, mask_inliers = \
seed_rt10_pair_kneip_eigensolver(q0,q1)
return rt10, mask_inliers
else:
# Reading Shehryar's poses
if i0 < 0:
return mrcal.identity_rt(), \
None
return rt_cam_camprev__from_data_file[i0], \
None
def optimizer_callback_triangulated_no_mrcal(
# shape (Nframes,6)
# this is ALL the poses, including for camera0
rt_ned_cam,
*,
# shape (Nframes, Nfeatures, 3)
v_all,
# shape (Nframes, Nfeatures); may be None
masks_valid = None,
# shape (Nframes_optimized,6, Nframes_optimized,6)
JtJ = None,
# shape (Nframes_optimized,6)
Jtx = None,
# list. Pass [] if we want to fill this in
x = None,
# list. Pass [] if we want to fill this in
J = None
):
Nframes = len(rt_ned_cam)
# Fill the off-diagonal entries
for i in range(Nframes-1):
for j in range(i+1,Nframes):
rtij, drtij_drti, drtij_drtj = \
mrcal.compose_rt( rt_ned_cam[i],
rt_ned_cam[j],
inverted0 = True,
get_gradients = True)
drij_dri = drtij_drti[:3,:3]
drij_dti = drtij_drti[:3,3:]
dtij_dri = drtij_drti[3:,:3]
dtij_dti = drtij_drti[3:,3:]
drij_drj = drtij_drtj[:3,:3]
drij_dtj = drtij_drtj[:3,3:]
dtij_drj = drtij_drtj[3:,:3]
dtij_dtj = drtij_drtj[3:,3:]
if masks_valid is not None:
# common observations between points i and j
mask_valid = masks_valid[i] * masks_valid[j]
# shape (Nfeatures_valid,3)
vi = v_all[i,...][...,mask_valid,:]
vj = v_all[j,...][...,mask_valid,:]
else:
vi = v_all[i,...]
vj = v_all[j,...]
# vj in the coord system of camera i
vj,dvj_drij,_ = \
mrcal.rotate_point_r(rtij[:3], vj,
get_gradients = True)
# shape (Nfeatures_valid,)
e,de_dvj,de_dtij = \
mrcal.triangulation._triangulated_error( vi, vj,
v_are_local = False,
t01 = rtij[3:],
get_gradients = True)
# p = mrcal.triangulate_leecivera_mid2( vi, vj,
# v_are_local = False,
# t01 = rtij[3:])
# print(p[:10])
# sys.exit()
# These are mostly for debugging
if x is not None:
x.append(e)
if J is not None:
Jhere = np.zeros((len(e), (Nframes-1), 6), dtype=float)
if i > 0:
Jhere[:,i-1,:] += nps.matmult(nps.dummy(de_dvj,-2), dvj_drij, drtij_drti[:3,:])[:,0,:]
Jhere[:,i-1,:] += nps.matmult(nps.dummy(de_dtij,-2), drtij_drti[3:,:])[:,0,:]
Jhere[:,j-1,:] += nps.matmult(nps.dummy(de_dvj,-2), dvj_drij, drtij_drtj[:3,:])[:,0,:]
Jhere[:,j-1,:] += nps.matmult(nps.dummy(de_dtij,-2), drtij_drtj[3:,:])[:,0,:]
J.append(Jhere)
if JtJ is None and Jtx is None:
continue
# shape (Nfeatures_valid,6)
de_dri = \
nps.matmult(nps.dummy(de_dtij, -2),
dtij_dri)[:,0,:] + \
nps.matmult(nps.dummy(de_dvj, -2),
dvj_drij,
drij_dri)[:,0,:]
# dtij_drj is 0
if np.any(dtij_drj): raise Exception("dtij_drj is expected to be 0")
de_dti = \
nps.matmult(nps.dummy(de_dtij, -2),
dtij_dti)[:,0,:]
de_dtj = \
nps.matmult(nps.dummy(de_dtij, -2),
dtij_dtj)[:,0,:]
de_drj = nps.matmult(nps.dummy(de_dvj, -2),
dvj_drij,
drij_drj)[:,0,:]
if np.any(drij_dti): raise Exception("drij_dti is expected to be 0")
if np.any(drij_dtj): raise Exception("drij_dtj is expected to be 0")
if i != 0:
# off-diagonal
if JtJ is not None:
# shape (6,6); we fill these in
JtJ_ij = JtJ[i-1,:,j-1,:]
JtJ_ji = JtJ[j-1,:,i-1,:]
JtJ_ij[:3,:3] += nps.matmult(de_dri.T, de_drj)
JtJ_ij[:3,3:] += nps.matmult(de_dri.T, de_dtj)
JtJ_ij[3:,:3] += nps.matmult(de_dti.T, de_drj)
JtJ_ij[3:,3:] += nps.matmult(de_dti.T, de_dtj)
JtJ_ji[:3,:3] += JtJ_ij[:3,:3].T
JtJ_ji[:3,3:] += JtJ_ij[3:,:3].T
JtJ_ji[3:,:3] += JtJ_ij[:3,3:].T
JtJ_ji[3:,3:] += JtJ_ij[3:,3:].T
# diagonal _i
JtJ_ii = JtJ[i-1,:,i-1,:]
JtJ_ii[:3,:3] += nps.matmult(de_dri.T, de_dri)
JtJ_ii[:3,3:] += nps.matmult(de_dri.T, de_dti)
JtJ_ii[3:,:3] += nps.matmult(de_dti.T, de_dri)
JtJ_ii[3:,3:] += nps.matmult(de_dti.T, de_dti)
if Jtx is not None:
Jtx[i-1,:3] += nps.matmult(e, de_dri)
Jtx[i-1,3:] += nps.matmult(e, de_dti)
# diagonal _j
if JtJ is not None:
JtJ_jj = JtJ[j-1,:,j-1,:]
JtJ_jj[:3,:3] += nps.matmult(de_drj.T, de_drj)
JtJ_jj[:3,3:] += nps.matmult(de_drj.T, de_dtj)
JtJ_jj[3:,:3] += nps.matmult(de_dtj.T, de_drj)
JtJ_jj[3:,3:] += nps.matmult(de_dtj.T, de_dtj)
if Jtx is not None:
Jtx[j-1,:3] += nps.matmult(e, de_drj)
Jtx[j-1,3:] += nps.matmult(e, de_dtj)
def solve_triangulated_no_mrcal(# shape (Nframes,6)
rt_ned_cam,
*,
# shape (Nframes, Nfeatures, 3)
v_all,
# shape (Nframes, Nfeatures)
masks_valid = None,
debug = False):
r'''I want a least-squares solve using implicit triangulation. I have a
measurement vector x containing pairwise reprojection errors from each pair
of cameras. The x01 contain all errors from observations common to cameras 0
and 1. Thus dx01/drt0 and dx01/drt1 are non-zero, but all other gradients of
x01 are 0. I define J01 = dx01/drt0 and J10 = dx01/drt1. Thus
[ J01 J10 ...]
J = [ J02 J20 ...]
[ J10 J21 ...]
[ ... ... ... ...]
and
JtJ = [ sum( Ji1t Ji1, i=...) ...]
[ J01t J10 ...]
[ J02t J20 ]
If I have N camera poses being optimized then JtJ has shape (6N,6N)
'''
Niterations = 10
Nframes = len(rt_ned_cam)
# frame0 is the reference
Nframes_optimized = Nframes-1
JtJ = np.zeros( (Nframes_optimized,6,
Nframes_optimized,6), dtype=float)
Jtx = np.zeros( (Nframes_optimized,6), dtype=float )
# last one is the final state
x_before_step = [[] for i in range(Niterations+1) ] # [[]] * N would produce 5 of the same identical list
rt_ned_cam_all = np.zeros((Niterations+1, Nframes,6), dtype=float)
rt_ned_cam_all[0,:] = rt_ned_cam
J = None if not debug else []
for i_iteration in range(Niterations):
optimizer_callback_triangulated_no_mrcal( # in
rt_ned_cam_all[i_iteration,...],
v_all = v_all,
# out
masks_valid = masks_valid,
JtJ = JtJ,
Jtx = Jtx,
x = x_before_step[i_iteration],
J = J)
# shape ( (Nframes-1)*6, (Nframes-1)*6)
JtJ_clumped = nps.clump( nps.clump(JtJ,
n = 2),
n = -2 )
# shape (Nframes-1)*6
Jtx_clumped = Jtx.ravel()
# error checking
if debug:
# shape (Nmeasurements, (Nframes-1)*6)
J = nps.clump( nps.glue(*J, axis=-3),
n = -2 )
# shape (Nmeasurements)
x0 = nps.glue(*x0, axis=-1)
Jtx_check = nps.matmult(x0, J)
if np.max(nps.norm2(Jtx_clumped - Jtx_check)) > 1e-12:
print("Jtx is wrong")
JtJ_check = nps.matmult(J.T, J)
if np.max(nps.norm2((JtJ_clumped - JtJ_check).ravel())) > 1e-12:
print("JtJ is wrong")
import gnuplotlib as gp
import IPython
IPython.embed()
sys.exit()
# ignore last state element. This will set our scale
Nstate = len(Jtx_clumped)
JtJ_clumped = JtJ_clumped[:Nstate-1, :Nstate-1]
Jtx_clumped = Jtx_clumped[:Nstate-1]
(F,lower) = scipy.linalg.cho_factor(JtJ_clumped)
drt_ned_cam = -scipy.linalg.cho_solve((F,lower), Jtx_clumped)
drt_ned_cam = nps.glue( 0,0,0,0,0,0,
drt_ned_cam, 0,
axis=-1).reshape(Nframes,6)
# take step
rt_ned_cam_all[i_iteration+1,...] = rt_ned_cam_all[i_iteration,...] + drt_ned_cam
# diagnostics
if True:
optimizer_callback_triangulated_no_mrcal( # in
rt_ned_cam_all[-1,...],
v_all = v_all,
# out
masks_valid = masks_valid,
x = x_before_step[-1])
import gnuplotlib as gp
# shape (Niterations+1, Nsensor_combinations,Nfeatures)
err_before_step = np.array(x_before_step)
norm2_E = nps.norm2( nps.clump(err_before_step,n=-2))
print(f"{norm2_E=}")
gp.plotimage(np.abs(nps.clump(err_before_step,n=2)), square=1)
import IPython
IPython.embed()
sys.exit()
return rt_ned_cam_all[-1,...]
print(f"dt = {t1-t0}s")
# mask_valid = mask_inbounds(q_all[0]) * mask_inbounds(q_all[0])
# Rt01, mask_bad, Nmask_bad = \
# pairwise_solve_kneip(*v_all[:,mask_valid,:])
import IPython
IPython.embed()
sys.exit()
def feature_matching__colmap(colmap_database_filename,
Nimages = None # None means "all the images"
):
r'''Read matches from a COLMAP database This isn't trivial to make, and I'm
not 100% sure what I did. I did "colmap gui" then "new project", then some
feature stuff. The output is only available in the opaque database with BLOBs
that this function tries to parse. I mostly reverse-engineered the format, but
there's documentation here:
https://colmap.github.io/database.html
I can also do a similar thing using alicevision:
av=$HOME/debianstuff/AliceVision/build/Linux-x86_64
$av/aliceVision_cameraInit --defaultFieldOfView 80 --imageFolder ~/data/xxxxx/delta -o xxxxx.sfm
$av/aliceVision_featureExtraction -p low -i xxxxx.sfm -o xxxxx-features
$av/aliceVision_featureMatching -i xxxxx.sfm -f xxxxx-features -o xxxxx-matches
$av/aliceVision_incrementalSfM -i xxxxx.sfm -f xxxxx-features -m xxxxx-matches -o xxxxx-sfm-output.ply
'''
db = sqlite3.connect(colmap_database_filename)
def parse_row(image_id, rows, cols, data):
return np.frombuffer(data, dtype=np.float32).reshape(rows,cols)
rows_keypoints = db.execute("SELECT * from keypoints")
keypoints = [ parse_row(*row) for row in rows_keypoints ]
rows_matches = db.execute("SELECT * from matches")
def get_correspondences_from_one_image_pair(row):
r'''Reports all the corresponding pixels in ONE pair of images
If we have N images, and all of them have some overlapping views, the we'll have
to make N*(N-1)/2 get_correspondences_from_one_image_pair() calls to get all the data
'''
pair_id, rows, cols, data = row
def pair_id_to_image_ids(pair_id):
r'''function from the docs
https://colmap.github.io/database.html
It reports 1-based indices. It also looks wrong: dividing by 0x7FFFFFFF is
WEIRD. But I guess that's what they did...
'''
image_id2 = pair_id % 2147483647
image_id1 = (pair_id - image_id2) // 2147483647
return image_id1, image_id2
def pair_id_to_image_ids__old(pair_id):
r'''function I reverse-engineered It reports 0-based indices. Agrees
with the one above for a few small numbers.
'''
i0 = (pair_id >> 31) - 1
i1 = (pair_id & 0x7FFFFFFF) + i0
return i0,i1
i0,i1 = pair_id_to_image_ids(pair_id)
# move their image indices to be 0-based
i0 -= 1
i1 -= 1
# shape (Ncorrespondences, 2)
# feature indices
f01 = np.frombuffer(data, dtype=np.uint32).reshape(rows,cols)
# shape (Nimages=2, Npoints, Nxy=2)
q = nps.cat(keypoints[i0][f01[:,0],:2],
keypoints[i1][f01[:,1],:2])
# shape (Npoints, Nimages=2, Nxy=2)
q = nps.xchg(q, 0,1)
# Convert colmap pixels to mrcal pixels. Colmap has the image origin at
# the top-left corner of the image, NOT at the center of the top-left
# pixel:
#
# https://colmap.github.io/database.html?highlight=convention#keypoints-and-descriptors
q -= 0.5
return (i0,
i1,
f01,
q.astype(float))
point_indices__from_image = dict()
ipoint_next = 0
def retrieve_cache(i,f):
nonlocal point_indices__from_image
# Return the point-index cache for image i. This indexes on feature
# indices f. It's possible that f contains out-of-bounds indices. In
# that case I grow my cache.
if i not in point_indices__from_image:
# New never-before-seen image. I initialize the cache, with each
# value being <0: I've never seen any of these point. I start out
# with an arbitrary number of features: 100
point_indices__from_image[i] = -np.ones((100,), dtype=int)
idx = point_indices__from_image[i]
maxf = np.max(f)
if maxf >= idx.size:
# I grow to double what I need now. I waste memory in order to
# reallocate less often
idx_new = -np.ones((maxf*2,), dtype=int)
idx_new[:idx.size] = idx
point_indices__from_image[i] = idx_new
idx = point_indices__from_image[i]
# I now have a big-enough cache. The caller can use it
return idx
def look_up_point_index(i0, i1, f01):
# i0, i1 are integer scalars: which images we're looking at
#
# f01 has shape (Npoints, Nimages=2). Integers identifying the point IN
# EACH IMAGE
f0 = f01[:,0]
f1 = f01[:,1]
# Each ipt_idx is a 1D numpy array. It's indexed by the f0,f1 feature
# indices. The value is a point index, or <0 if it hasn't been seen yet
ipt_idx0 = retrieve_cache(i0, f0)
ipt_idx1 = retrieve_cache(i1, f1)
# The point indices
idx0 = ipt_idx0[f0]
idx1 = ipt_idx1[f1]
# - If an index is found in only one cache, I add it to the other cache.
#
# - If an index is found in BOTH caches, it should refer to the same
# point. If it doesn't I need to rethink this
#
# - If an index is not found in either cache, I make a new point index,
# and add it to both caches
idx_found0 = idx0 >= 0
idx_found1 = idx1 >= 0
if np.any(idx_found0 * idx_found1):
raise Exception("I encountered an observation, and I've seen both of these points already. I think this shouldn't happen? It did happen, though. So I should make sure that both of these refer to the same point. I'm not implementing that yet because I don't think this will ever actually happen")
# copy the found indices
idx1[idx_found0] = idx0[idx_found0]
idx0[idx_found1] = idx1[idx_found1]
# Now I make new indices for newly-observed points
nonlocal ipoint_next
idx_not_found = ~idx_found0 * ~idx_found1
Npoints_new = np.count_nonzero(idx_not_found)
idx0[idx_not_found] = np.arange(Npoints_new) + ipoint_next
idx1[idx_not_found] = np.arange(Npoints_new) + ipoint_next
ipoint_next += Npoints_new
# Everything is cached and done. I can look up the point indices for
# this call
if np.any(idx0 - idx1):
raise Exception("Point index mismatch. This is a bug")
return idx0
Nobservations_max = 1000000
indices_point_camintrinsics_camextrinsics_pool = \
np.zeros((Nobservations_max,3),
dtype = np.int32)
observations_pool = \
np.ones((Nobservations_max,3),
dtype = float)
Nobservations = 0
for row in rows_matches:
# i0, i1 are scalars
# q has shape (Npoints, Nimages=2, Nxy=2)
# f01 has shape (Npoints, Nimages=2)
i0,i1,f01,q = get_correspondences_from_one_image_pair(row)
if Nimages is not None and \
(i0 >= Nimages or \
i1 >= Nimages):
continue
# shape (Npoints,)
ipt = look_up_point_index(i0,i1,f01)
Nobservations_here = q.shape[-3] * 2
if Nobservations + Nobservations_here > Nobservations_max:
raise Exception("Exceeded Nobservations_max")
# pixels observed from the first camera
iobservation0 = range(Nobservations,
Nobservations+Nobservations_here//2)
# pixels observed from the second camera
iobservation1 = range(Nobservations+Nobservations_here//2,
Nobservations+Nobservations_here)
indices_point_camintrinsics_camextrinsics_pool[iobservation0, 0] = ipt
indices_point_camintrinsics_camextrinsics_pool[iobservation1, 0] = ipt
# [iobservation, 1] is already 0: I'm moving around a single camera
# -1 because camera0 defines my coord system, and is not present in the
# -extrinsics vector
indices_point_camintrinsics_camextrinsics_pool[iobservation0, 2] = i0-1
indices_point_camintrinsics_camextrinsics_pool[iobservation1, 2] = i1-1
observations_pool[iobservation0, :2] = q[:,0,:]
observations_pool[iobservation1, :2] = q[:,1,:]
# [iobservation1,2] already has weight=1.0
Nobservations += Nobservations_here
indices_point_camintrinsics_camextrinsics = \
indices_point_camintrinsics_camextrinsics_pool[:Nobservations]
observations = \
observations_pool[:Nobservations]
# I resort the observations to cluster them by points, as (currently; for
# now?) required by mrcal. I want a stable sort to preserve the camera
# sorting order within each point. This isn't strictly required, but makes
# it easier to think about
iobservations = \
np.argsort(indices_point_camintrinsics_camextrinsics[:,0],
kind = 'stable')
indices_point_camintrinsics_camextrinsics[:] = \
indices_point_camintrinsics_camextrinsics[iobservations,...]
observations[:] = \
observations[iobservations,...]
return \
indices_point_camintrinsics_camextrinsics, \
observations
feature_finder = None
matcher = None
def feature_matching__opencv(i_image, image0_decimated, image1_decimated):
import cv2
global feature_finder, matcher
if feature_finder is None:
feature_finder = cv2.ORB_create()
matcher = cv2.BFMatcher(cv2.NORM_HAMMING,
crossCheck = True)
if 1:
# shape (Hdecimated,Wdecimated,2)
flow_decimated = \
cv2.calcOpticalFlowFarneback(image0_decimated, image1_decimated,
flow = None, # for in-place output
pyr_scale = 0.5,
levels = 3,
winsize = 15,
iterations = 3,
poly_n = 5,
poly_sigma = 1.2,
flags = 0# cv2.OPTFLOW_USE_INITIAL_FLOW
)
else:
import IPython
IPython.embed()
sys.exit()
# shape (Hdecimated,Wdecimated,2)
flow_decimated = \
cv2.optflow.calcOpticalFlowSF(image0_decimated, image1_decimated,
flow = None, # for in-place output
pyr_scale = 0.5,
levels = 3,
winsize = 15,
iterations = 3,
poly_n = 5,
poly_sigma = 1.2,
flags = 0# cv2.OPTFLOW_USE_INITIAL_FLOW
)
# 'calcOpticalFlowDenseRLOF',
# 'calcOpticalFlowSF',
# 'calcOpticalFlowSparseRLOF',
# 'calcOpticalFlowSparseToDense',
# 'createOptFlow_DeepFlow',
# 'createOptFlow_DenseRLOF',
# 'createOptFlow_DualTVL1',
# 'createOptFlow_Farneback',
# 'createOptFlow_PCAFlow',
# 'createOptFlow_SimpleFlow',
# 'createOptFlow_SparseRLOF',
# 'createOptFlow_SparseToDense'
if 1:
plot_flow(image0_decimated, flow_decimated,
decimation_extra_plot,
hardcopy = f"{outdir}/flow{i_image:03d}.png")
keypoints0, descriptors0 = feature_finder.detectAndCompute(image0_decimated, None)
keypoints1, descriptors1 = feature_finder.detectAndCompute(image1_decimated, None)
matches = matcher.match(descriptors0, descriptors1)
# shape (Nmatches, Npair=2, Nxy=2)
qall_decimated = nps.cat(*[np.array((keypoints0[m.queryIdx].pt,
keypoints1[m.trainIdx].pt)) \
for m in matches])
i_match_valid = \
np.array([i for i in range(len(matches)) \
if match_looks_valid(qall_decimated[i],
matches[i],
flow_decimated[int(round(qall_decimated[i][0,1])),
int(round(qall_decimated[i][0,0]))]
)])
if len(i_match_valid) < 10:
raise Exception(f"Too few valid features found: N = {len(i_match_valid)}")
return \
decimation * qall_decimated[i_match_valid]
def show_matched_features(image0_decimated, image1_decimated, q):
##### feature-matching visualizations
if 0:
# Plot two sets of points: one for each image in the pair
gp.plot( # shape (Npair=2, Nmatches, Nxy=2)
nps.xchg(q,0,1),
legend = np.arange(2),
_with='points',
tuplesize=-2,
square=1,
_xrange = (0,W),
_yrange = (H,0),
hardcopy='/tmp/tst.gp')
sys.exit()
elif 0:
# one plot, with connected lines: vertical stacking
gp.plot( (nps.glue(image0_decimated,image1_decimated,
axis=-2),
dict( _with = 'image', \
tuplesize = 3 )),
(q/decimation + np.array(((0,0), (0,image0_decimated.shape[-2])),),
dict( _with = 'lines',
tuplesize = -2)),
_set = ('xrange noextend',
'yrange noextend reverse',
'palette gray'),
square=1,
hardcopy='/tmp/tst.gp')
sys.exit()
elif 0:
# one plot, with connected lines: horizontal stacking
gp.plot( (nps.glue(image0_decimated,image1_decimated,
axis=-1),
dict( _with = 'image', \
tuplesize = 3 )),
(q/decimation + np.array(((0,0), (image0_decimated.shape[-1],0)),),
dict( _with = 'lines',
tuplesize = -2)),
_set = ('xrange noextend',
'yrange noextend reverse',
'palette gray'),
square=1,
hardcopy='/tmp/tst.gp')
sys.exit()
elif 0:
# two plots. Discrete points
images_decimated = (image0_decimated,
image1_decimated)
g = [None] * 2
pids = set()
for i in range(2):
g[i] = gp.gnuplotlib(_set = ('xrange noextend',
'yrange noextend reverse',
'palette gray'),
square=1)
g[i].plot( (images_decimated[i],
dict( _with = 'image', \
tuplesize = 3 )),
(q[:,(i,),:]/decimation,
dict( _with = 'points',
tuplesize = -2,
legend = i_match_valid)))
pid = os.fork()
if pid == 0:
# child
g[i].wait()
sys.exit(0)
pids.add(pid)
# wait until all the children finish
while len(pids):
pid,status = os.wait()
if pid in pids:
pids.remove(pid)
def get_observation_pair(i0, i1,
indices_point_camintrinsics_camextrinsics,
observations):
# i0, i1 are indexed from -1: these are the camextrinsics indices
if i0+1 != i1:
raise Exception("get_observation_pair() currently only works for consecutive indices")
# The data is clumped by points. I'm looking at
# same-point-consecutive-camera observations, so they're guaranteed to
# appear consecutively
mask_cam0 = indices_point_camintrinsics_camextrinsics[:,2] == i0
mask_cam0[-1] = False # I'm going to be looking at the "next" row, so I
# ignore the last row, since there's no "next" one
# after it
idx_cam0 = np.nonzero(mask_cam0)[0]
row_cam0 = \
indices_point_camintrinsics_camextrinsics[idx_cam0]
row_next = \
indices_point_camintrinsics_camextrinsics[idx_cam0+1]
# I care about corresponding rows that represent the same point and my two
# cameras
idx_cam0_selected = \
idx_cam0[(row_cam0[:,0] == row_next[:,0]) * (row_next[:,2] == i1)]
q0 = observations[idx_cam0_selected, :2]
q1 = observations[idx_cam0_selected+1, :2]
return q0,q1
def mark_outliers_from_seed(i0, i1,
optimization_inputs,
mask_correspondence_outliers):
# Very similar to get_observation_pair(). Please consolidate
indices_point_camintrinsics_camextrinsics = optimization_inputs['indices_point_triangulated_camintrinsics_camextrinsics']
observations = optimization_inputs['observations_point_triangulated']
# i0, i1 are indexed from -1: these are the camextrinsics indices
if i0+1 != i1:
raise Exception("mark_outliers_from_seed() currently only works for consecutive indices")
# The data is clumped by points. I'm looking at
# same-point-consecutive-camera observations, so they're guaranteed to
# appear consecutively
mask_cam0 = indices_point_camintrinsics_camextrinsics[:,2] == i0
mask_cam0[-1] = False # I'm going to be looking at the "next" row, so I
# ignore the last row, since there's no "next" one
# after it
idx_cam0 = np.nonzero(mask_cam0)[0]
row_cam0 = \
indices_point_camintrinsics_camextrinsics[idx_cam0]
row_next = \
indices_point_camintrinsics_camextrinsics[idx_cam0+1]
# I care about corresponding rows that represent the same point and my two
# cameras
idx_cam0_selected = \
idx_cam0[(row_cam0[:,0] == row_next[:,0]) * (row_next[:,2] == i1)]
print(f"i0 = {i0}. marking {np.count_nonzero(mask_correspondence_outliers)} correspondences as outliers. 2x observations: {2*np.count_nonzero(mask_correspondence_outliers)}")
iobservation0 = (idx_cam0_selected )[mask_correspondence_outliers]
iobservation1 = (idx_cam0_selected+1)[mask_correspondence_outliers]
observations[iobservation0, 2] = -1.
observations[iobservation1, 2] = -1.
def mark_outliers(indices_point_camintrinsics_camextrinsics, observations,
rt_cam_ref):
# I consider consecutive observations only. So if a single point was
# observed by in-order frames 0,3,4,6 then here I will consider (0,3), (3,4)
# and (4,6)
ipoint_current = -1
iobservation0 = -1
vlocal = mrcal.unproject(observations[:,:2], *model.intrinsics(),
normalize = True)
cos_half_theta_threshold = np.cos(1. * np.pi/180. / 2.)
for iobservation1 in range(len(indices_point_camintrinsics_camextrinsics)):
ipoint,icami,icame = indices_point_camintrinsics_camextrinsics[iobservation1]
weight = observations[iobservation1, 2]
if weight <= 0:
# This observation is an outlier. There's nothing at all to do
continue
if ipoint == ipoint_current and \
iobservation0 >= 0:
# I'm observing the same point as the previous observation. Compare
# them
ipoint_prev,icami_prev,icame_prev = \
indices_point_camintrinsics_camextrinsics[iobservation0]
if icame_prev >= 0: rt0r = rt_cam_ref[icame_prev]
else: rt0r = mrcal.identity_rt()
if icame >= 0: rt01 = mrcal.compose_rt(rt0r, mrcal.invert_rt(rt_cam_ref[icame]))
else: rt01 = rt0r
v0 = vlocal[iobservation0]
v1 = mrcal.rotate_point_r(rt01[:3], vlocal[iobservation1])
t01 = rt01[3:]
p = mrcal.triangulate_leecivera_mid2(v0,v1,t01)
if nps.norm2(p) == 0 or \
nps.inner(p, v0) / nps.mag(p) < cos_half_theta_threshold:
# outlier
observations[iobservation0,2] = -1.
observations[iobservation1,2] = -1.
iobservation0 = -1
continue
# I KEEP GOING, BUT THIS IS A BUG. What about previous
# observations of this point that were deemed not an outlier?
# Are those still good? I should also set iobservation0 to
# the previous observation in this point that wasn't an outlier.
# The logic is complicated so I just ignore it for the time
# being
else:
# This is a different point from the previous. Or the previous point
# was an outlier. The next iteration will compare against this one
ipoint_current= ipoint
iobservation0 = iobservation1
def solve(indices_point_camintrinsics_camextrinsics,
observations,
Nimages):
observations_fixed = None
indices_point_camintrinsics_camextrinsics_fixed = None
points_fixed = None
Npoints_fixed = 0
if 0:
if 1:
# delta. desert
# hard-coded known at-infinity point seen in the first two cameras
q0_infinity = np.array((1853,1037), dtype=float)
q1_infinity = np.array((1920,1039), dtype=float)
else:
# xxxxx ranch
q0_infinity = np.array((2122, 501), dtype=float)
q1_infinity = np.array((1379, 548), dtype=float)
observations_fixed = nps.glue( q0_infinity,
q1_infinity,
axis = -2 )
p_infinity = \
10000 * \
mrcal.unproject(q0_infinity, *model.intrinsics(),
normalize = True)
indices_point_camintrinsics_camextrinsics_fixed = \
np.array((( 0, 0, -1),
( 0, 0, 0),),
dtype = np.int32)
# weights
observations_fixed = nps.glue( observations_fixed,
np.ones((2,1), dtype=np.int32),
axis = -1)
points_fixed = nps.atleast_dims(p_infinity, -2)
Npoints_fixed = 1
# i0 is indexed from -1: it's a camextrinsics index
#
# This is a relative array:
# [ rt10 ]
# [ rt21 ]
# [ rt32 ]
# [ .... ]
seed_rt10_and_mask_correspondences_inliers = \
[seed_rt10_pair( i0+1,
*get_observation_pair(i0, i0+1,
indices_point_camintrinsics_camextrinsics,
observations) ) \
for i0 in range(-1,Nimages-2)]
rt_cam_camprev = \
np.array([rm[0] for rm in seed_rt10_and_mask_correspondences_inliers])
# Make an absolute extrinsics array:
# [ rt10 ]
# [ rt20 ]
# [ rt30 ]
# [ .... ]
rt_cam_ref = np.zeros(rt_cam_camprev.shape, dtype=float)
rt_cam_ref[0] = rt_cam_camprev[0]
for i in range(1,len(rt_cam_camprev)):
rt_cam_ref[i] = mrcal.compose_rt( rt_cam_camprev[i],
rt_cam_ref[i-1] )
optimization_inputs = \
dict( intrinsics = nps.atleast_dims(model.intrinsics()[1], -2),
rt_cam_ref = nps.atleast_dims(rt_cam_ref, -2),
observations_point_triangulated = observations,
indices_point_triangulated_camintrinsics_camextrinsics = indices_point_camintrinsics_camextrinsics,
points = points_fixed,
indices_point_camintrinsics_camextrinsics = indices_point_camintrinsics_camextrinsics_fixed,
observations_point = observations_fixed,
Npoints_fixed = Npoints_fixed,
point_min_range = 1.,
point_max_range = 20000.,
lensmodel = model.intrinsics()[0],
imagersizes = nps.atleast_dims(model.imagersize(), -2),
do_optimize_intrinsics_core = False,
do_optimize_intrinsics_distortions = False,
do_optimize_extrinsics = True,
do_optimize_frames = True,
do_apply_outlier_rejection = True,
do_apply_regularization = True,
do_apply_regularization_unity_cam01 = True,
verbose = True)
# I injest whatever outliers I got from the seeding algorithm
for i0 in range(-1,Nimages-2):
mask_correspondence_inliers = seed_rt10_and_mask_correspondences_inliers[i0+1][1]
if mask_correspondence_inliers is None:
continue
mark_outliers_from_seed(i0, i0+1,
optimization_inputs,
~mask_correspondence_inliers)
# And now another pass of pre-solve outlier rejection. As currently
# implemented, the seeding only considers consecutive-frame observations.
# Any observations of a point in frames 0,2 that are NOT observed in frame 1
# are not considered by the seeding algorithm, and any outliers will not
# appear. Here I go through each observation
mark_outliers(indices_point_camintrinsics_camextrinsics, observations,
rt_cam_ref)
print(f"Seed rt_cam_ref = {rt_cam_ref}")
write_models("/tmp/xxxxx-seed-cam{}.cameramodel",
model,
optimization_inputs['rt_cam_ref'])
stats = mrcal.optimize(**optimization_inputs)
write_models("/tmp/xxxxx-solve-cam{}.cameramodel",
model,
optimization_inputs['rt_cam_ref'])
print(f"Solved rt_cam_ref = {optimization_inputs['rt_cam_ref']}")
x = stats['x']
return x, optimization_inputs
def show_solution(optimization_inputs, Nimages, binary = True):
def write_points(f, p, bgr):
N = len(p)
if binary:
binary_ply = np.empty( (N,),
dtype = dtype)
binary_ply['xyz'] = p
binary_ply['rgba'][:,0] = bgr[:,2]
binary_ply['rgba'][:,1] = bgr[:,1]
binary_ply['rgba'][:,2] = bgr[:,0]
binary_ply['rgba'][:,3] = 255
binary_ply.tofile(f)
else:
fp = nps.glue(p,
bgr[:,(2,)],
bgr[:,(1,)],
bgr[:,(0,)],
255*np.ones((N,1)),
axis = -1)
np.savetxt(f,
fp,
fmt = ('%.1f','%.1f','%.1f','%d','%d','%d','%d'),
comments = '',
header = '')
return N
if binary:
ply_type = 'binary_little_endian'
else:
ply_type = 'ascii'
placeholder = 'NNNNNNN'
ply_header = f'''ply
format {ply_type} 1.0
element vertex {placeholder}
property float x
property float y
property float z
property uchar red
property uchar green
property uchar blue
property uchar alpha
end_header
'''.encode()
Npoints_pointcloud = 0
filename_point_cloud = "/tmp/points.ply"
with open(filename_point_cloud, 'wb') as f:
dtype = np.dtype([ ('xyz',np.float32,3), ('rgba', np.uint8, 4) ])
f.write(ply_header)
# Camera positions in red
rt_cam_ref = nps.glue( np.zeros((6,)),
optimization_inputs['rt_cam_ref'],
axis = -2 )
t_ref_cam = mrcal.invert_rt(rt_cam_ref)[:,3:]
write_points(f,
t_ref_cam,
np.zeros((Nimages,3), dtype=np.uint8) +
np.array((0,0,255), dtype=np.uint8))
Npoints_pointcloud += Nimages
# Here I only look at consecutive image pairs, even though the
# optimization looked at ALL the pairs
for i0 in range(-1, Nimages-2):
i1 = i0+1
q0, q1 = \
get_observation_pair(i0, i1,
optimization_inputs['indices_point_triangulated_camintrinsics_camextrinsics'],
optimization_inputs['observations_point_triangulated'])
if i0 < 0:
rt_0r = mrcal.identity_rt()
rt_01 = mrcal.invert_rt(optimization_inputs['rt_cam_ref'][i1])
else:
rt_0r = optimization_inputs['rt_cam_ref'][i0]
rt_1r = optimization_inputs['rt_cam_ref'][i1]
rt_01 = mrcal.compose_rt( rt_0r,
mrcal.invert_rt(rt_1r) )
v0 = mrcal.unproject(q0, *model.intrinsics())
v1 = mrcal.unproject(q1, *model.intrinsics())
plocal0 = \
mrcal.triangulate_leecivera_mid2(v0, v1,
v_are_local = True,
Rt01 = mrcal.Rt_from_rt(rt_01))
r = nps.mag(plocal0)
index_good_triangulation = r > 0
q0_r_recip = nps.glue(q0[index_good_triangulation],
nps.transpose(1./r[index_good_triangulation]),
axis = -1)
# filename_overlaid_points = f'/tmp/overlaid-points-{i0+1}.pdf'
# gp.plot(q0_r_recip,
# tuplesize = -3,
# _with = 'points pt 7 ps 0.5 palette',
# square = 1,
# _xrange = (0,W),
# _yrange = (H,0),
# rgbimage = image_filename[i0+1],
# hardcopy = filename_overlaid_points,
# cbmax = 0.1)
# print(f"Wrote '{filename_overlaid_points}'")
image = mrcal.load_image(image_filename[i0+1], bits_per_pixel = 24, channels = 3)
######### point cloud
#### THIS IS WRONG: I report a separate point in each consecutive
#### triangulation, so if I tracked a feature over N frames, instead
#### of reporting one point for that feature, I'll report N-1 of
#### them
# I'm including the alpha byte to align each row to 16 bytes.
# Otherwise I have unaligned 32-bit floats. I don't know for a fact
# that this breaks anything, but it feels like it would maybe.
i = (q0[index_good_triangulation] + 0.5).astype(int)
bgr = image[i[:,1], i[:,0]]
Npoints_pointcloud += \
write_points(f,
mrcal.transform_point_rt(mrcal.invert_rt(rt_0r),
plocal0[index_good_triangulation]),
bgr)
# I wrote the point cloud file with an unknown number of points. Now that I
# have the count, I go back, and fill it in.
import mmap
with open(filename_point_cloud, 'r+b') as f:
m = mmap.mmap(f.fileno(), 0)
i_placeholder_start = ply_header.find(placeholder.encode())
placeholder_width = ply_header[i_placeholder_start:].find(b'\n')
i_placeholder_end = i_placeholder_start + placeholder_width
m[i_placeholder_start:i_placeholder_end] = \
'{:{width}d}'.format(Npoints_pointcloud, width=placeholder_width).encode()
m.close()
print(f"Wrote '{filename_point_cloud}'")
def write_model(filename, model):
print(f"Writing '{filename}'")
model.write(filename)
def write_models(filename_format,
model_baseline, rt_cam_ref):
model0 = mrcal.cameramodel(model_baseline)
model0.rt_cam_ref(np.zeros((6,), dtype=float))
write_model(filename_format.format(0), model0)
for i in range(1,len(rt_cam_ref)+1):
model1 = mrcal.cameramodel(model_baseline)
model1.rt_cam_ref(rt_cam_ref[i-1])
write_model(filename_format.format(i), model1)
if __name__ == "__main__":
if 1:
# delta. desert
image_glob = "/home/dima/data/xxxxx/delta/*.jpg"
outdir = "/tmp"
decimation = 20
decimation_extra_plot = 5
model_filename = "/home/dima/xxxxx-sfm/cam.cameramodel"
colmap_database_filename = 'xxxxx.exhaustive.db'
image_filename = sorted(glob.glob(image_glob))
else:
# xxxxx ranch
# t1_t2_p_qxyzw = np.loadtxt("/mnt/nvm/xxxxx-xxxxx-ranch/time_stamp_xyz_xyzw.vnl",
# dtype = [ ('time', np.uint64),
# ('timestamp', np.uint64),
# ('p', float, (3,)),
# ('quat_xyzw', float, (4,)),])
# t_filename = np.loadtxt("/mnt/nvm/xxxxx-xxxxx-ranch/time_filename.vnl",
# dtype = [ ('timestamp', np.uint64),
# ('filename', 'S50') ])
# quat_xyzw = t1_t2_p_qxyzw['quat_xyzw']
# quat = quat_xyzw[...,(3,0,1,2)]
# r = mrcal.r_from_R( mrcal.R_from_quat(quat) )
# rt_ref_veh_all = nps.glue(r,
# t1_t2_p_qxyzw['p'],
# axis = -1)
t_dt_p_qxyzw = np.loadtxt("/mnt/nvm/xxxxx-xxxxx-ranch/relative-poses.vnl",
dtype = float)
t_filename = np.loadtxt("/mnt/nvm/xxxxx-xxxxx-ranch/time_filename.vnl",
dtype = [ ('timestamp', np.uint64),
('filename', 'S250') ])
quat_xyzw = t_dt_p_qxyzw[:,5:]
quat = quat_xyzw[...,(3,0,1,2)]
r = mrcal.r_from_R( mrcal.R_from_quat(quat) )
rt_cam0_cam1_all = nps.glue(r,
t_dt_p_qxyzw[:,2:5],
axis = -1)
# Row i in the pose file has
# t[i]-t[i-1] == dt[i]
# So I presume it has rt_camprev_cam
#
# I also checked, and the timestamps in t_filename match those in
# t_dt_p_qxyzw exactly. No "interpolation" is needed, but I'll ask for it
# anyway
import scipy.interpolate
f = \
scipy.interpolate.interp1d(t_dt_p_qxyzw[:,0],
rt_cam0_cam1_all,
axis = -2,
bounds_error = True,
assume_sorted = True)
# I want the last 7 images
image_filename = t_filename['filename' ][-7:]
rt_camprev_cam__from_data_file = f(t_filename['timestamp'][-7:].astype(float) / 1e9)
# The first image doesn't have a camprev. Throw it away
rt_camprev_cam__from_data_file = rt_camprev_cam__from_data_file[1:]
rt_cam_camprev__from_data_file = mrcal.invert_rt(rt_camprev_cam__from_data_file)
image_dir = "/mnt/nvm/xxxxx-xxxxx-ranch/images-last10"
outdir = "/tmp"
decimation = 20
decimation_extra_plot = 5
model_filename = "/mnt/nvm/xxxxx-xxxxx-ranch/oryx.cameramodel"
colmap_database_filename = '/mnt/nvm/xxxxx-xxxxx-ranch/xxxxx.db'
image_filename = [ f"{image_dir}/{os.path.basename(f.decode())}" for f in image_filename]
model = mrcal.cameramodel(model_filename)
W,H = model.imagersize()
Nimages = 6
# q.shape = (Npoints, Nimages=2, Nxy=2)
if 0:
image0,image0_decimated = imread(image_filename[0], decimation)
image1,image1_decimated = imread(image_filename[1], decimation)
q = feature_matching__opencv(0, image0_decimated, image1_decimated)
show_matched_features(image0_decimated, image1_decimated, q)
else:
indices_point_camintrinsics_camextrinsics, \
observations = \
feature_matching__colmap(colmap_database_filename,
Nimages)
x,optimization_inputs = solve(indices_point_camintrinsics_camextrinsics,
observations,
Nimages)
show_solution(optimization_inputs, Nimages)
import IPython
IPython.embed()
sys.exit()
try:
image0_decimated = image1_decimated
except:
# if I'm looking at cached features, I never read any actual images
pass
# test code to observe the behavior of mrcal.triangulation._triangulated_error()
if False:
import gnuplotlib as gp
q0 = (np.array((W,H), dtype=float) - 1.) / 2.
Nx = 100
Ny = 9
q1x = np.linspace(q0[0]-20, q0[0]+20, Nx)
q1y = np.linspace(q0[1]-10, q0[1]+10, Ny)
# shape (Ny,Nx,2)
q1 = np.ascontiguousarray(nps.mv( nps.cat(*np.meshgrid(q1x,q1y)),
0, -1))
v0 = mrcal.unproject(q0, *model.intrinsics())
v1 = mrcal.unproject(q1, *model.intrinsics())
e = \
mrcal.triangulation._triangulated_error( v0, v1,
v_are_local = True,
Rt01 = mrcal.Rt_from_rt( np.array((0,0,0, 1.,0,0))) )
gp.plot(q1x,e,
legend=q1y,
hardcopy='/tmp/tst.gp')
sys.exit()
mrcal-2.5/analyses/splines/ 0000775 0000000 0000000 00000000000 15123677724 0015770 5 ustar 00root root 0000000 0000000 mrcal-2.5/analyses/splines/README 0000664 0000000 0000000 00000003347 15123677724 0016657 0 ustar 00root root 0000000 0000000 The test and validation tools used in the development of the b-spline
interpolated models live here.
First I wanted to come up with a surface parametrization scheme. I wanted to
project the full almost-360deg view into some sort of 2D domain that is
cartesian-ish. Then I would grid this cartesian 2D domain with a regular grid of
control points, and I'd spline that. But does such a mapping exist? What is it?
The
show-fisheye-grid.py
script exists for this purpose. It takes in an existing model, grids the imager,
unprojects the points, and projects the vectors back to a number of common
fisheye projections. I applied this to some wide models I had (their inaccurate
OPENCV8 fits), and observed that the stereographic model works well here: a
cartensian grid in the imager produces a cartesian-ish grid in the stereographic
projection. The other projections look like they'd work also. The pinhole
projection most notably does NOT work; its projections run off to infinity at
the edges.
Then I needed to find a surface representation, and the expressions. I'm using
b-splines: they have nice continuity properties, and they have nice local
support: so my jacobian will remain sparse. The
bsplines.py
script derives and validates the spline equations. fpbisp.py is a part of that.
Then I implemented the spline equations in mrcal.c, and wanted to make sure the
behavior was correct. This is done by the
verify-interpolated-surface.py
script. It produces a random splined model, projects it with mrcal, and
visualizes the sparse control point surface with the dense sampled surface. The
sampled surface should appear smooth, and should be guided by the control
points. This tool is the first test that the mrcal projection is implemented
correctly
mrcal-2.5/analyses/splines/bsplines.py 0000775 0000000 0000000 00000037100 15123677724 0020165 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Studies the 2d surface b-spline interpolation
The expressions for the interpolated surface I need aren't entirely clear, so I
want to study this somewhat. This script does that.
I want to use b-splines to drive the generic camera models. These provide
local support in the control points at the expense of not interpolating the
control points. I don't NEED an interpolating spline, so this is just fine.
Let's assume the knots lie at integer coordinates.
I want a function that takes in
- the control points in a neighborhood of the spline segment
- the query point x, scaled to [0,1] in the spline segment
This is all nontrivial for some reason, and there're several implementations
floating around with slightly different inputs. I have
- sample_segment_cubic()
From a generic calibration research library:
https://github.com/puzzlepaint/camera_calibration/blob/master/applications/camera_calibration/scripts/derive_jacobians.py
in the EvalUniformCubicBSpline() function. This does what I want (query
point in [0,1], control points around it, one segment at a time), but the
source of the expression isn't given. I'd like to re-derive it, and then
possibly extend it
- splev_local()
wraps scipy.interpolate.splev()
- splev_translated()
Followed sources of scipy.interpolate.splev() to the core fortran functions
in fpbspl.f and splev.f. I then ran these through f2c, pythonified it, and
simplified it. The result is sympy-able
- splev_wikipedia()
Sample implementation of De Boor's algorithm from wikipedia:
https://en.wikipedia.org/wiki/De_Boor%27s_algorithm
from EvalUniformCubicBSpline() in
camera_calibration/applications/camera_calibration/scripts/derive_jacobians.py.
translated to [0,1] from [3,4]
'''
import sys
import numpy as np
import numpysane as nps
import gnuplotlib as gp
skip_plots = False
import scipy.interpolate
from scipy.interpolate import _fitpack
def sample_segment_cubic(x, a,b,c,d):
A = (-x**3 + 3*x**2 - 3*x + 1)/6
B = (3 * x**3/2 - 3*x**2 + 2)/3
C = (-3 * x**3 + 3*x**2 + 3*x + 1)/6
D = (x * x * x) / 6
return A*a + B*b + C*c + D*d
def splev_local(x, t, c, k, der=0, ext=0):
y = scipy.interpolate.splev(x, (t,c,k), der, ext)
return y.reshape(x.shape)
def splev_translated(x, t, c, k, l):
# assumes that t[l] <= x <= t[l+1]
# print(f"l = {l}")
# print((t[l], x, t[l+1]))
l += 1 # l is now a fortran-style index
hh = [0] * 19
h__ = [0] * 19
h__[-1 + 1] = 1
i__1 = k
for j in range(1,i__1+1):
i__2 = j
for i__ in range(1,i__2+1):
hh[-1 + i__] = h__[-1 + i__]
h__[-1 + 1] = 0
i__2 = j
for i__ in range(1,i__2+1):
li = l + i__
lj = li - j
if t[-1 + li] != t[-1 + lj]:
h__[-1 + i__] += (t[-1 + li] - x) * hh[-1 + i__] / (t[-1 + li] - t[-1 + lj])
h__[-1 + i__ + 1] = (x - t[-1 + lj]) * hh[-1 + i__] / (t[-1 + li] - t[-1 + lj])
else:
h__[-1 + i__ + 1] = 0
sp = 0
ll = l - (k+1)
i__2 = (k+1)
for j in range(1,i__2+1):
ll += 1
sp += c[-1 + ll] * h__[-1 + j]
return sp
# from https://en.wikipedia.org/wiki/De_Boor%27s_algorithm
def splev_wikipedia(k: int, x: int, t, c, p: int):
"""Evaluates S(x).
Arguments
---------
k: Index of knot interval that contains x.
x: Position.
t: Array of knot positions, needs to be padded as described above.
c: Array of control points.
We will look at c[k-p .. k]
p: Degree of B-spline.
"""
# make sure I never reference c out of bounds
if k-p < 0: raise Exception("c referenced out of min bounds")
if k >= len(c): raise Exception("c referenced out of max bounds")
d = [c[j + k - p] for j in range(0, p+1)]
for r in range(1, p+1):
for j in range(p, r-1, -1):
alpha = (x - t[j+k-p]) / (t[j+1+k-r] - t[j+k-p])
d[j] = (1 - alpha) * d[j-1] + alpha * d[j]
return d[p]
##############################################################################
# First I confirm that the functions from numpy and from wikipedia produce the
# same results. I don't care about edge cases for now. I query an arbitrary
# point
N = 30
t = np.arange( N, dtype=int)
k = 3
c = np.random.rand(len(t))
x = 4.3
# may have trouble exactly AT the knots. the edge logic isn't quite what I want
l = np.searchsorted(np.array(t), x)-1
y0 = splev_local(np.array((x,)),t,c,k)[0]
y1 = splev_translated(x, t, c, k, l)
y2 = splev_wikipedia(l, x, t, c, k)
print(f"These should all match: {y0} {y1} {y2}")
print(f" err1 = {y1-y0}")
print(f" err2 = {y2-y0}")
##############################################################################
# OK, good. I want to make sure that the spline roughly follows the curve
# defined by the control points. There should be no x offset or anything of that
# sort
N = 30
t = np.arange( N, dtype=int)
k = 3
c = np.random.rand(len(t))
Npad = 10
x = np.linspace(Npad, N-Npad, 1000)
########### sample_segment_cubic()
@nps.broadcast_define(((),), ())
def sample_cubic(x, cp):
i = int(x//1)
q = x-i
try: return sample_segment_cubic(q, *cp[i-1:i+3])
except: return 0
y = sample_cubic(x, c)
c2 = c.copy()
c2[int(N//2)] *= 1.1
y2 = sample_cubic(x, c2)
if not skip_plots:
plot1 = gp.gnuplotlib(title = 'Cubic splines: response to control point perturbation',
# hardcopy = '/tmp/cubic-spline-perturbations.svg',
# terminal = 'svg size 800,600 noenhanced solid dynamic font ",12"'
)
plot1.plot( (x, nps.cat(y,y2),
dict(_with = np.array(('lines lc "blue"',
'lines lc "sea-green"')),
legend = np.array(('Spline: baseline',
'Spline: tweaked one control point')))),
(t[:len(c)], nps.cat(c,c2),
dict(_with = np.array(('points pt 1 ps 1 lc "blue"',
'points pt 2 ps 1 lc "sea-green"')),
legend= np.array(('Control points: baseline',
'Control points: tweaked one control point')))),
(x, y-y2,
dict(_with = 'lines lc "red"',
legend = 'Difference',
y2 = 1)),
_xrange = (10.5,19.5),
y2max = 0.01,
ylabel = 'Spline value',
y2label = 'Difference due to perturbation',)
########### sample_wikipedia()
@nps.broadcast_define(((),), ())
def sample_wikipedia(x, t, c, k):
return splev_wikipedia(np.searchsorted(np.array(t), x)-1,x,t,c,k)
@nps.broadcast_define(((),), ())
def sample_wikipedia_integer_knots(x, c, k):
t = np.arange(len(c) + k, dtype=int)
l = int(x//1)
offset = int((k+1)//2)
return splev_wikipedia(l,x,t,c[offset:],k)
if 1:
y = sample_wikipedia_integer_knots(x,c,k)
else:
offset = int((k+1)//2)
y = sample_wikipedia(x,t, c[offset:], k)
if not skip_plots:
plot2 = gp.gnuplotlib(title = 'sample_wikipedia')
plot2.plot( (x, y, dict(_with='lines')),
(t[:len(c)], c, dict(_with='linespoints pt 7 ps 2')) )
print("these two plots should look the same: we're using two implementation of the same algorithm to interpolate the same data")
plot2.wait()
plot1.wait()
# Now I use sympy to get the polynomial coefficients from sample_wikipedia.
# These should match the ones in sample_segment_cubic()
import sympy
c = sympy.symbols(f'c:{len(c)}')
x = sympy.symbols('x')
# Which interval we're in. Arbitrary. In the middle somewhere
l = int(N//2)
print("Should match A,B,C,D coefficients in sample_segment_cubic()")
s = splev_wikipedia(l,
# I want 'x' to be [0,1] within the interval, but this
# function wants x in the whole domain
x+l,
np.arange( N, dtype=int), c, k).expand()
print(s.coeff(c[12]))
print(s.coeff(c[13]))
print(s.coeff(c[14]))
print(s.coeff(c[15]))
print("Should also match A,B,C,D coefficients in sample_segment_cubic()")
s = splev_translated(# I want 'x' to be [0,1] within the interval, but this
# function wants x in the whole domain
x+l,
np.arange( N, dtype=int), c, k, l).expand()
print(s.coeff(c[12]))
print(s.coeff(c[13]))
print(s.coeff(c[14]))
print(s.coeff(c[15]))
#########################################################
# Great. More questions. Here I have a cubic spline (k==3). And to evaluate the
# spline value I need to have 4 control point values available, 2 on either
# side. Piecewise-linear splines are too rough, but quadratic splines could work
# (k==2). What do the expressions look like? How many neighbors do I need? Here
# the control point values c represent the function value between adjacent
# knots, so each segment uses 3 neighboring control points, and is defined in a
# region [-0.5..0.5] off the center control point
print("======================== k = 2")
N = 30
t = np.arange( N, dtype=int)
k = 2
# c[0,1,2] corresponds to is t[-0.5 0.5 1.5 2.5 ...
c = np.random.rand(len(t))
x = 4.3
# may have trouble exactly AT the knots. the edge logic isn't quite what I want
l = np.searchsorted(np.array(t), x)-1
y0 = splev_local(np.array((x,)),t,c,k)[0]
y1 = splev_translated(x, t, c, k, l)
y2 = splev_wikipedia(l, x, t, c, k)
print(f"These should all match: {y0} {y1} {y2}")
print(f" err1 = {y1-y0}")
print(f" err2 = {y2-y0}")
##############################################################################
# OK, good. I want to make sure that the spline roughly follows the curve
# defined by the control points. There should be no x offset or anything of that
# sort
if not skip_plots:
N = 30
t = np.arange( N, dtype=int)
k = 2
c = np.random.rand(len(t))
Npad = 10
x = np.linspace(Npad, N-Npad, 1000)
offset = int((k+1)//2)
y = sample_wikipedia(x,t-0.5, c[offset:], k)
xm = (x[1:] + x[:-1]) / 2.
d = np.diff(y) / np.diff(x)
plot1 = gp.gnuplotlib(title = 'k==2; sample_wikipedia')
plot1.plot( (x, y, dict(_with='lines', legend='spline')),
(xm, d, dict(_with='lines', y2=1, legend='diff')),
(t[:len(c)], c, dict(_with='linespoints pt 7 ps 2', legend='control points')))
@nps.broadcast_define(((),), ())
def sample_splev_translated(x, t, c, k):
l = np.searchsorted(np.array(t), x)-1
return splev_translated(x,t,c,k,l)
y = sample_splev_translated(x,t-0.5, c[offset:], k)
xm = (x[1:] + x[:-1]) / 2.
d = np.diff(y) / np.diff(x)
plot2 = gp.gnuplotlib(title = 'k==2; splev_translated')
plot2.plot( (x, y, dict(_with='lines', legend='spline')),
(xm, d, dict(_with='lines', y2=1, legend='diff')),
(t[:len(c)], c, dict(_with='linespoints pt 7 ps 2', legend='control points')))
# These are the functions I'm going to use. Derived by the sympy steps
# immediately after this
def sample_segment_quadratic(x, a,b,c):
A = (4*x**2 - 4*x + 1)/8
B = (3 - 4*x**2)/4
C = (4*x**2 + 4*x + 1)/8
return A*a + B*b + C*c
@nps.broadcast_define(((),), ())
def sample_quadratic(x, cp):
i = int((x+0.5)//1)
q = x-i
try: return sample_segment_quadratic(q, *cp[i-1:i+2])
except: return 0
y = sample_quadratic(x, c)
xm = (x[1:] + x[:-1]) / 2.
d = np.diff(y) / np.diff(x)
plot3 = gp.gnuplotlib(title = 'k==2; sample_quadratic')
plot3.plot( (x, y, dict(_with='lines', legend='spline')),
(xm, d, dict(_with='lines', y2=1, legend='diff')),
(t[:len(c)], c, dict(_with='linespoints pt 7 ps 2', legend='control points')))
plot3.wait()
plot2.wait()
plot1.wait()
print("these 3 plots should look the same: we're using different implementation of the same algorithm to interpolate the same data")
# ##################################
# # OK, these match. Let's get the expression of the polynomial in a segment. This
# # was used to construct sample_segment_quadratic() above
# c = sympy.symbols(f'c:{len(c)}')
# x = sympy.symbols('x')
# l = int(N//2)
# print("A,B,C for k==2 using splev_wikipedia()")
# s = splev_wikipedia(l,
# # I want 'x' to be [-0.5..0.5] within the interval, but this
# # function wants x in the whole domain
# x+l,
# np.arange( N, dtype=int) - sympy.Rational(1,2), c, k).expand()
# print(s)
# print(s.coeff(c[13]))
# print(s.coeff(c[14]))
# print(s.coeff(c[15]))
# # I see this:
# # c13*x**2/2 - c13*x/2 + c13/8 - c14*x**2 + 3*c14/4 + c15*x**2/2 + c15*x/2 + c15/8
# # x**2/2 - x/2 + 1/8
# # 3/4 - x**2
# # x**2/2 + x/2 + 1/8
############## compare cubic, quadratic
# I now have nice expressions for quadratic and cubic interpolations. Both are
# C1 continuous, but the cubic interpolation is C2 continuous too. How much do I
# care? Let's at least compare them visually
N = 30
t = np.arange( N, dtype=int)
c = np.random.rand(len(t))
Npad = 10
x = np.linspace(Npad, N-Npad, 1000)
y2 = sample_quadratic(x,c)
y3 = sample_cubic( x,c)
if not skip_plots:
gp.plot( (x, nps.cat(y2,y3), dict(_with='lines',
legend=np.array(('quadratic',
'cubic')))),
(t[:len(c)], c, dict(_with='linespoints pt 7 ps 2',
legend='control points')),
title = "Comparing quadratic and cubic b-spline interpolation",
wait = True)
# Visually, neither looks better than the other. The quadratic spline follows
# the control points closer. Looks like it's trying to match the slope at the
# halfway point. Maybe that's bad, and the quadratic curve is too wiggly? We'll
# see
####################################################################
# Great. Final set of questions: how do you we make a 2D spline surface? The
# generic calibration research library
# (https://github.com/puzzlepaint/camera_calibration) Does a set of 1d
# interpolations in one dimension, and then interpolates the 4 interpolated
# values along the other dimension. Questions:
#
# Does order matter? I can do x and then y or y then x
#
# And is there a better way? scipy has 2d b-spline interpolation routines. Do
# they do something better?
#
# ############### x-y or y-x?
import sympy
from sympy.abc import x,y
cp = sympy.symbols('cp:4(:4)')
# x then y
xs = [sample_segment_cubic( x,
cp[i*4 + 0],
cp[i*4 + 1],
cp[i*4 + 2],
cp[i*4 + 3] ) for i in range(4)]
yxs = sample_segment_cubic(y, *xs)
# y then x
ys = [sample_segment_cubic( y,
cp[0*4 + i],
cp[1*4 + i],
cp[2*4 + i],
cp[3*4 + i] ) for i in range(4)]
xys = sample_segment_cubic(x, *ys)
print(f"Bicubic interpolation. x-then-y and y-then-x difference: {(xys - yxs).expand()}")
########### Alright. Apparently either order is ok. Does scipy do something
########### different for 2d interpolation? I compare the 1d-then-1d
########### interpolation above to fitpack results:
from fpbisp import fpbisp_
N = 30
t = np.arange( N, dtype=int)
k = 3
cp = np.array(sympy.symbols('cp:40(:40)'), dtype=np.object).reshape(40,40)
lx = 3
ly = 5
z = fpbisp_(t, t, k, cp, x+lx, lx, y+ly, ly)
err = z - yxs
print(f"Bicubic interpolation. difference(1d-then-1d, FITPACK): {err.expand()}")
# Apparently chaining 1D interpolations produces identical results to what FITPACK is doing
mrcal-2.5/analyses/splines/fpbisp.py 0000664 0000000 0000000 00000003156 15123677724 0017632 0 ustar 00root root 0000000 0000000 r'''Translation of fpbpl and fpbisp from Fortran to Python
The scipy source includes FITPACK, which implements b-spline interpolation. I
converted its 2D surface interpolation routine (fpbisp) to C (via f2c), and then
semi-manually to Python. I can then feed it sympy symbols, and get out
analytical expressions, which are surprisingly difficult to find in a nice
usable form on the internet.
The original fpbisp implementation is at
scipy/interpolate/fitpack/fpbisp.f
The analysis that uses this conversion lives in bsplines.py
'''
import sympy
def fpbspl_(t, k, x, l):
h__ = [None] * 100
hh = [None] * 100
h__[-1 + 1] = sympy.Integer(1)
i__1 = k
for j in range(1,i__1+1):
i__2 = j
for i__ in range(1,i__2+1):
hh[i__ - 1] = h__[-1 + i__]
h__[-1 + 1] = 0
i__2 = j
for i__ in range(1,i__2+1):
li = l + i__
lj = li - j
f = hh[i__ - 1] / (t[li] - t[lj])
h__[-1 + i__] += f * (t[li] - x)
h__[-1 + i__ + 1] = f * (x - t[lj])
return h__
# argx is between tx[lx] and tx[lx+1]. Same with y
def fpbisp_(tx, ty, k, c, argx, lx, argy, ly):
wx = [None] * 100
wy = [None] * 100
h__ = fpbspl_(tx, k, argx, lx)
for j in range(1, (k+1)+1):
wx[1 + j] = h__[j - 1]
h__ = fpbspl_(ty, k, argy, ly)
for j in range(1,(k+1)+1):
wy[1 + j] = h__[j - 1]
for i1 in range(1,(k+1)+1):
h__[i1 - 1] = wx[1 + i1]
sp = 0
for i1 in range(1,(k+1)+1):
for j1 in range(1,(k+1)+1):
sp += c[j1-1,i1-1] * h__[i1 - 1] * wy[1 + j1]
return sp
mrcal-2.5/analyses/splines/show-fisheye-grid.py 0000775 0000000 0000000 00000010442 15123677724 0021703 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Evaluate 2D griddings of a camera view
I'm interested in gridding the observation vectors for projection. These
observation vectors have length-1, so they're 3D quantities that live in a 2D
manifold. I need some sort of a 2D representation so that I can convert between
this representation and the 3D vectors without worrying about constraints.
I'd like a rectangular 2D gridding of observation vectors to more-or-less map to
a gridding of projected pixel coordinates: I want both to be as rectangular as
possible. This tool grids the imager, and plots the unprojected grid for a
number of different 3d->2d schemes.
I take in an existing camera mode, and I grid its imager. I unproject each
pixel, and reproject back using the scheme I'm looking at. Most schemes are
fisheye projections described at
https://en.wikipedia.org/wiki/Fisheye_lens
mrcal-show-distortion-off-pinhole --radial also compares a given model with fisheye
projections
'''
import sys
import argparse
import re
import os
def parse_args():
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('scheme',
type=str,
choices=('pinhole', 'stereographic', 'equidistant', 'equisolidangle', 'orthographic'),
help='''Type of fisheye model to visualize. For a description of the choices see
https://en.wikipedia.org/wiki/Fisheye_lens''')
parser.add_argument('model',
type=str,
help='''Camera model to grid''')
args = parser.parse_args()
return args
args = parse_args()
# arg-parsing is done before the imports so that --help works without building
# stuff, so that I can generate the manpages and README
import numpy as np
import numpysane as nps
import gnuplotlib as gp
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/../..",
import mrcal
@nps.broadcast_define(((3,),), (2,))
def project_simple(v, d):
k = d[4:]
fxy = d[:2]
cxy = d[2:4]
x,y = v[:2]/v[2]
r2 = x*x + y*y
r4 = r2*r2
r6 = r4*r2
a1 = 2*x*y
a2 = r2 + 2*x*x
a3 = r2 + 2*y*y
cdist = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6
icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6)
return np.array(( x*cdist*icdist2 + k[2]*a1 + k[3]*a2,
y*cdist*icdist2 + k[2]*a3 + k[3]*a1 )) * fxy + cxy
@nps.broadcast_define(((3,),), (2,))
def project_radial_numdenom(v, d):
k = d[4:]
fxy = d[:2]
cxy = d[2:4]
x,y = v[:2]/v[2]
r2 = x*x + y*y
r4 = r2*r2
r6 = r4*r2
a1 = 2*x*y
a2 = r2 + 2*x*x
a3 = r2 + 2*y*y
return np.array((1 + k[0]*r2 + k[1]*r4 + k[4]*r6,
1 + k[5]*r2 + k[6]*r4 + k[7]*r6))
try:
m = mrcal.cameramodel(args.model)
except:
print(f"Couldn't read '{args.model}' as a camera model", file=sys.stderr)
sys.exit(1)
W,H = m.imagersize()
Nw = 40
Nh = 30
# shape (Nh,Nw,2)
xy = \
nps.mv(nps.cat(*np.meshgrid( np.linspace(0,W-1,Nw),
np.linspace(0,H-1,Nh) )),
0,-1)
fxy = m.intrinsics()[1][0:2]
cxy = m.intrinsics()[1][2:4]
# shape (Nh,Nw,2)
v = mrcal.unproject(np.ascontiguousarray(xy), *m.intrinsics())
v0 = mrcal.unproject(cxy, *m.intrinsics())
# shape (Nh,Nw)
costh = nps.inner(v,v0) / (nps.mag(v) * nps.mag(v0))
th = np.arccos(costh)
# shape (Nh,Nw,2)
xy_rel = xy-cxy
# shape (Nh,Nw)
az = np.arctan2( xy_rel[...,1], xy_rel[..., 0])
if args.scheme == 'stereographic': r = np.tan(th/2.) * 2.
elif args.scheme == 'equidistant': r = th
elif args.scheme == 'equisolidangle': r = np.sin(th/2.) * 2.
elif args.scheme == 'orthographic': r = np.sin(th)
elif args.scheme == 'pinhole': r = np.tan(th)
else: print("Unknown scheme {args.scheme}. Shouldn't happen. argparse should have taken care of it")
mapped = xy_rel * nps.dummy(r/nps.mag(xy_rel),-1)
gp.plot(mapped, tuplesize=-2,
_with = 'linespoints',
title = f"Gridded model '{args.model}' looking at pinhole unprojection with z=1",
xlabel = f'Normalized {args.scheme} x',
ylabel = f'Normalized {args.scheme} y',
square = True,
wait = True)
mrcal-2.5/analyses/splines/verify-interpolated-surface.py 0000775 0000000 0000000 00000011151 15123677724 0023766 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
f'''Observe the interpolation grid implemented in the C code
This is a validation of mrcal.project()
'''
import sys
import os
import numpy as np
import numpysane as nps
import gnuplotlib as gp
testdir = os.path.dirname(os.path.realpath(__file__))
# I import the LOCAL mrcal since that's what I'm testing
sys.path[:0] = f"{testdir}/../..",
import mrcal
order = 3
Nx = 11
Ny = 8
fov_x_deg = 200 # more than 180deg
imagersize = np.array((3000,2000))
fxy = np.array((2000., 1900.))
cxy = (imagersize.astype(float) - 1.) / 2.
# I want random control points, with some (different) bias on x and y
controlpoints_x = np.random.rand(Ny, Nx) * 1 + 0.5 * np.arange(Nx)/Nx
controlpoints_y = np.random.rand(Ny, Nx) * 1 - 0.9 * np.arange(Nx)/Nx
# to test a delta function
# controlpoints_y*=0
# controlpoints_y[4,5] = 1
# The parameters vector dimensions appear (in order from slowest-changing to
# fastest-changing):
# - y coord
# - x coord
# - fx/fy
parameters = nps.glue( np.array(( fxy[0], fxy[1], cxy[0], cxy[1])),
nps.mv(nps.cat(controlpoints_x,controlpoints_y),
0, -1).ravel(),
axis = -1 )
# Now I produce a grid of observation vectors indexed on the coords of the
# control-point arrays
# The index into my spline. ixy has shape (Ny,Nx,2) and contains (x,y) rows
Nw = Nx*5
Nh = Ny*5
x_sampled = np.linspace(1,Nx-2,Nh)
y_sampled = np.linspace(1,Ny-2,Nw)
ixy = \
nps.mv( nps.cat(*np.meshgrid( x_sampled, y_sampled )),
0,-1)
##### this has mostly been implemented in mrcal_project_stereographic() and
##### mrcal_unproject_stereographic()
# Stereographic projection function:
# p = xyz
# rxy = mag(xy)
# th = atan2(rxy, z)
# u = tan(th/2) * 2. * xy/mag(xy)
# q = (u + deltau) * fxy + cxy
#
# I look up deltau in the splined surface. The index of that lookup is linear
# (and cartesian) with u
#
# So ixy = u * k + (Nxy-1)/2
#
# ix is in [0,Nx] (modulo edges). one per control point
#
# Note that the same scale k is applied for both the x and y indices. The
# constants set the center of the spline surface to x=0 and y=0
#
# The scale k is set by fov_x_deg (this is at y=0):
#
# ix_margin = tan(-fov_x_deg/2/2) * 2. * k + (Nx-1)/2 --->
# k = (ix_margin - (Nx-1)/2) / (tan(fov_x_deg/2/2) * 2)
#
# I want to compute p from (ix,iy). p is unique up-to scale. So let me
# arbitrarily set mag(xy) = 1. I define a new var
#
# jxy = tan(th/2) xy --->
# jxy = (ixy - (Nxy-1)/2) / (2k)
#
# jxy = tan(th/2) xy
# = (1 - cos(th)) / sin(th) xy
# = (1 - cos(atan2(1, z))) / sin(atan2(1, z)) xy
# = (1 - z/mag(xyz)) / (1/mag(xyz)) xy =
# = (mag(xyz) - z) xy =
#
# mag(jxy) = (mag(xyz) - z)
# = sqrt(z^2+1) - z
#
# Let h = sqrt(z^2+1) + z ->
# mag(jxy) h = 1
# h - mag(jxy) = 2z
# ---> z = (h - mag(jxy)) / 2 = (1/mag(jxy) - mag(jxy)) / 2
if order == 3:
# cubic splines. There's exactly one extra control point on each side past
# my fov. So ix_margin = 1
ix_margin = 1
else:
# quadratic splines. 1/2 control points on each side past my fov
ix_margin = 0.5
k = (ix_margin - (Nx-1)/2) / (np.tan(-fov_x_deg*np.pi/180./2/2) * 2)
jxy = (ixy - (np.array( (Nx, Ny), dtype=float) - 1.)/2.) / k / 2.
mjxy = nps.mag(jxy)
z = (1./mjxy - mjxy) / 2.
xy = jxy / nps.dummy(mjxy, -1) # singular at the center. Do I care?
p = nps.glue(xy, nps.dummy(z,-1), axis=-1)
mxy = nps.mag(xy)
# Bam. I have applied a stereographic unprojection to get 3D vectors that would
# stereographically project to given spline grid locations. I use the mrcal
# internals to project the unprojection, and to get the focal lengths it ended
# up using. If the internals were implemented correctly, the dense surface of
# focal lengths should follow the sparse surface of spline control points
lensmodel_type = f'LENSMODEL_SPLINED_STEREOGRAPHIC_order={order}_Nx={Nx}_Ny={Ny}_fov_x_deg={fov_x_deg}'
q = mrcal.project(np.ascontiguousarray(p), lensmodel_type, parameters)
th = np.arctan2( nps.mag(p[..., :2]), p[..., 2])
uxy = p[..., :2] * nps.dummy(np.tan(th/2)*2/nps.mag(p[..., :2]), -1)
deltau = (q-cxy) / fxy - uxy
deltaux,deltauy = nps.mv(deltau, -1,0)
gp.plot3d( (deltauy,
dict( _with='lines',
using=f'($1*{x_sampled[1]-x_sampled[0]}+{x_sampled[0]}):($2*{y_sampled[1]-y_sampled[0]}+{y_sampled[0]}):3' )),
(controlpoints_y,
dict( _with='points pt 7 ps 2' )),
xlabel='x control point index',
ylabel='y control point index',
title='Deltau-y',
squarexy=True,
ascii=True,
wait=True)
mrcal-2.5/analyses/triangulation/ 0000775 0000000 0000000 00000000000 15123677724 0017173 5 ustar 00root root 0000000 0000000 mrcal-2.5/analyses/triangulation/study.py 0000775 0000000 0000000 00000027716 15123677724 0020735 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
r'''Study the precision and accuracy of the various triangulation routines'''
import sys
import argparse
import re
import os
def parse_args():
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--Nsamples',
type=int,
default=100000,
help='''How many random samples to evaluate. 100000 by
default''')
group = parser.add_mutually_exclusive_group(required = True)
group.add_argument('--ellipses',
action='store_true',
help='''Display the ellipses and samples in the xy plane''')
group.add_argument('--ranges',
action='store_true',
help='''Display the distribution of the range''')
parser.add_argument('--samples',
action='store_true',
help='''If --ellipses, plot the samples ALSO. Usually
this doesn't clarify anything, so the default is to omit
them''')
parser.add_argument('--cache',
type=str,
choices=('read','write'),
help=f'''A cache file stores the recalibration results;
computing these can take a long time. This option allows
us to or write the cache instead of sampling. The cache
file is hardcoded to a cache file (in /tmp). By default,
we do neither: we don't read the cache (we sample
instead), and we do not write it to disk when we're
done. This option is useful for tests where we reprocess
the same scenario repeatedly''')
parser.add_argument('--observed-point',
type = float,
nargs = 3,
default = ( 5000., 300., 2000.),
help='''The camera0 coordinate of the observed point.
Default is ( 5000., 300., 2000.)''')
parser.add_argument('--title',
type=str,
default = None,
help='''Title string for the plot. Overrides the default
title. Exclusive with --extratitle''')
parser.add_argument('--extratitle',
type=str,
default = None,
help='''Additional string for the plot to append to the
default title. Exclusive with --title''')
parser.add_argument('--hardcopy',
type=str,
help='''Write the output to disk, instead of an interactive plot''')
parser.add_argument('--terminal',
type=str,
help=r'''gnuplotlib terminal. The default is good almost always, so most people don't
need this option''')
parser.add_argument('--set',
type=str,
action='append',
help='''Extra 'set' directives to gnuplotlib. Can be given multiple times''')
parser.add_argument('--unset',
type=str,
action='append',
help='''Extra 'unset' directives to gnuplotlib. Can be given multiple times''')
args = parser.parse_args()
if args.title is not None and \
args.extratitle is not None:
print("--title and --extratitle are exclusive", file=sys.stderr)
sys.exit(1)
return args
args = parse_args()
import numpy as np
import numpysane as nps
import gnuplotlib as gp
import pickle
import os.path
# I import the LOCAL mrcal
scriptdir = os.path.dirname(os.path.realpath(__file__))
sys.path[:0] = f"{scriptdir}/../..",
import mrcal
############ bias visualization
#
# I simulate pixel noise, and see what that does to the triangulation. Play with
# the geometric details to get a sense of how these behave
model0 = mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE',
np.array((1000., 1000., 500., 500.))),
imagersize = np.array((1000,1000)) )
model1 = mrcal.cameramodel( intrinsics = ('LENSMODEL_PINHOLE',
np.array((1100., 1100., 500., 500.))),
imagersize = np.array((1000,1000)) )
# square camera layout
t01 = np.array(( 1., 0.1, -0.2))
R01 = mrcal.R_from_r(np.array((0.001, -0.002, -0.003)))
Rt01 = nps.glue(R01, t01, axis=-2)
p = np.array(args.observed_point)
q0 = mrcal.project(p, *model0.intrinsics())
sigma = 0.1
cache_file = "/tmp/triangulation-study-cache.pickle"
if args.cache is None or args.cache == 'write':
v0local_noisy, v1local_noisy,v0_noisy,v1_noisy,_,_,_,_ = \
mrcal.synthetic_data. \
_noisy_observation_vectors_for_triangulation(p,Rt01,
model0.intrinsics(),
model1.intrinsics(),
args.Nsamples,
sigma = sigma)
p_sampled_geometric = mrcal.triangulate_geometric( v0_noisy, v1_noisy, t01 )
p_sampled_lindstrom = mrcal.triangulate_lindstrom( v0local_noisy, v1local_noisy, Rt01 )
p_sampled_leecivera_l1 = mrcal.triangulate_leecivera_l1( v0_noisy, v1_noisy, t01 )
p_sampled_leecivera_linf = mrcal.triangulate_leecivera_linf( v0_noisy, v1_noisy, t01 )
p_sampled_leecivera_mid2 = mrcal.triangulate_leecivera_mid2( v0_noisy, v1_noisy, t01 )
p_sampled_leecivera_wmid2 = mrcal.triangulate_leecivera_wmid2(v0_noisy, v1_noisy, t01 )
q0_sampled_geometric = mrcal.project(p_sampled_geometric, *model0.intrinsics())
q0_sampled_lindstrom = mrcal.project(p_sampled_lindstrom, *model0.intrinsics())
q0_sampled_leecivera_l1 = mrcal.project(p_sampled_leecivera_l1, *model0.intrinsics())
q0_sampled_leecivera_linf = mrcal.project(p_sampled_leecivera_linf, *model0.intrinsics())
q0_sampled_leecivera_mid2 = mrcal.project(p_sampled_leecivera_mid2, *model0.intrinsics())
q0_sampled_leecivera_wmid2 = mrcal.project(p_sampled_leecivera_wmid2, *model0.intrinsics())
range_sampled_geometric = nps.mag(p_sampled_geometric)
range_sampled_lindstrom = nps.mag(p_sampled_lindstrom)
range_sampled_leecivera_l1 = nps.mag(p_sampled_leecivera_l1)
range_sampled_leecivera_linf = nps.mag(p_sampled_leecivera_linf)
range_sampled_leecivera_mid2 = nps.mag(p_sampled_leecivera_mid2)
range_sampled_leecivera_wmid2 = nps.mag(p_sampled_leecivera_wmid2)
if args.cache is not None:
with open(cache_file,"wb") as f:
pickle.dump((v0local_noisy,
v1local_noisy,
v0_noisy,
v1_noisy,
p_sampled_geometric,
p_sampled_lindstrom,
p_sampled_leecivera_l1,
p_sampled_leecivera_linf,
p_sampled_leecivera_mid2,
p_sampled_leecivera_wmid2,
q0_sampled_geometric,
q0_sampled_lindstrom,
q0_sampled_leecivera_l1,
q0_sampled_leecivera_linf,
q0_sampled_leecivera_mid2,
q0_sampled_leecivera_wmid2,
range_sampled_geometric,
range_sampled_lindstrom,
range_sampled_leecivera_l1,
range_sampled_leecivera_linf,
range_sampled_leecivera_mid2,
range_sampled_leecivera_wmid2),
f)
print(f"Wrote cache to {cache_file}")
else:
with open(cache_file,"rb") as f:
(v0local_noisy,
v1local_noisy,
v0_noisy,
v1_noisy,
p_sampled_geometric,
p_sampled_lindstrom,
p_sampled_leecivera_l1,
p_sampled_leecivera_linf,
p_sampled_leecivera_mid2,
p_sampled_leecivera_wmid2,
q0_sampled_geometric,
q0_sampled_lindstrom,
q0_sampled_leecivera_l1,
q0_sampled_leecivera_linf,
q0_sampled_leecivera_mid2,
q0_sampled_leecivera_wmid2,
range_sampled_geometric,
range_sampled_lindstrom,
range_sampled_leecivera_l1,
range_sampled_leecivera_linf,
range_sampled_leecivera_mid2,
range_sampled_leecivera_wmid2) = \
pickle.load(f)
plot_options = {}
if args.set is not None:
plot_options['set'] = args.set
if args.unset is not None:
plot_options['unset'] = args.unset
if args.hardcopy is not None:
plot_options['hardcopy'] = args.hardcopy
if args.terminal is not None:
plot_options['terminal'] = args.terminal
if args.ellipses:
# Plot the reprojected pixels and the fitted ellipses
data_tuples = \
[ *mrcal.utils._plot_args_points_and_covariance_ellipse( q0_sampled_geometric, 'geometric' ),
*mrcal.utils._plot_args_points_and_covariance_ellipse( q0_sampled_lindstrom, 'lindstrom' ),
*mrcal.utils._plot_args_points_and_covariance_ellipse( q0_sampled_leecivera_l1, 'lee-civera-l1' ),
*mrcal.utils._plot_args_points_and_covariance_ellipse( q0_sampled_leecivera_linf, 'lee-civera-linf' ),
*mrcal.utils._plot_args_points_and_covariance_ellipse( q0_sampled_leecivera_mid2, 'lee-civera-mid2' ),
*mrcal.utils._plot_args_points_and_covariance_ellipse( q0_sampled_leecivera_wmid2,'lee-civera-wmid2' ), ]
if not args.samples:
# Not plotting samples. Get rid of all the "dots" I'm plotting
data_tuples = [ t for t in data_tuples if \
not (isinstance(t[-1], dict) and \
'_with' in t[-1] and \
t[-1]['_with'] == 'dots') ]
if args.title is not None:
title = args.title
else:
title = 'Reprojected triangulated point'
if args.extratitle is not None:
title += ': ' + args.extratitle
gp.plot( *data_tuples,
( q0,
dict(_with = 'points pt 3 ps 2',
tuplesize = -2,
legend = 'Ground truth')),
square = True,
wait = 'hardcopy' not in plot_options,
title = title,
**plot_options)
elif args.ranges:
# Plot the range distribution
range_ref = nps.mag(p)
if args.title is not None:
title = args.title
else:
title = "Range distribution"
if args.extratitle is not None:
title += ': ' + args.extratitle
gp.plot( nps.cat( range_sampled_geometric,
range_sampled_lindstrom,
range_sampled_leecivera_l1,
range_sampled_leecivera_linf,
range_sampled_leecivera_mid2,
range_sampled_leecivera_wmid2 ),
legend = np.array(( 'range_sampled_geometric',
'range_sampled_lindstrom',
'range_sampled_leecivera_l1',
'range_sampled_leecivera_linf',
'range_sampled_leecivera_mid2',
'range_sampled_leecivera_wmid2' )),
histogram=True,
binwidth=200,
_with='lines',
_set = f'arrow from {range_ref},graph 0 to {range_ref},graph 1 nohead lw 5',
wait = 'hardcopy' not in plot_options,
title = title,
**plot_options)
else:
raise Exception("Getting here is a bug")
mrcal-2.5/analyses/validate-input-noise.py 0000775 0000000 0000000 00000005723 15123677724 0020740 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
# 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
r'''Validate deriving the input noise from the solve residuals
SYNOPSIS
$ validate-input-noise.py [01].cameramodel
Noise ratios measured/actual. Should be ~ 1.0
observed, by looking at the distribution of residulas: 0.998
predicted, by correcting the above by sqrt(1-Nstates/Nmeasurements_observed): 1.000
'''
import sys
import argparse
import re
import os
def parse_args():
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--observed-pixel-uncertainty',
type = float,
default = 0.3,
help='''How much noise to inject into perfect solves''')
parser.add_argument('models',
type=str,
nargs='+',
help='''The camera models to process. Each is handled
individually''')
return parser.parse_args()
args = parse_args()
# I import the LOCAL mrcal
sys.path[:0] = f"{os.path.dirname(os.path.realpath(__file__))}/..",
import mrcal
import mrcal.model_analysis
import numpy as np
import numpysane as nps
models = [mrcal.cameramodel(f) for f in args.models]
for model in models:
optimization_inputs = model.optimization_inputs()
mrcal.make_perfect_observations(optimization_inputs,
observed_pixel_uncertainty = args.observed_pixel_uncertainty)
mrcal.optimize(**optimization_inputs)
# The ratio of observed noise to what I expected. Should be ~ 1.0
noise_observed_ratio = \
mrcal.model_analysis._observed_pixel_uncertainty_from_inputs(optimization_inputs) / args.observed_pixel_uncertainty
Nstates = mrcal.num_states(**optimization_inputs)
Nmeasurements = mrcal.num_measurements(**optimization_inputs)
# This correction is documented here:
# https://mrcal.secretsauce.net/docs-3.0/formulation.html#estimating-input-noise
# This probably should be added to
# _observed_pixel_uncertainty_from_inputs(). Today (2025/11/15) it has not
# yet been. Because very're usually VERY overdetermined, which can be
# validated by this script. I will add this factor later, if I discover that this is necessary
f = np.sqrt(1 - Nstates/Nmeasurements)
noise_predicted_ratio = noise_observed_ratio/f
print("Noise ratios measured/actual. Should be ~ 1.0")
print(f" observed, by looking at the distribution of residulas: {noise_observed_ratio:.3f}")
print(f" predicted, by correcting the above by sqrt(1-Nstates/Nmeasurements_observed): {noise_predicted_ratio:.3f}")
mrcal-2.5/analyses/validate-noncentral.py 0000775 0000000 0000000 00000035332 15123677724 0020630 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
# 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
r'''Study the noncentral effects of this solve
SYNOPSIS
$ validate-noncentral.py [01].cameramodel
... plots pop up, showing the effect of removing points that are close to the
... lens. If they were causing poor fits due to noncentrality, we'd see
improved ... cross-validation
I take the optimization_inputs as they are, WITHOUT making perfect data, and I
re-solve the problem after throwing out points the percentile nearest
points. If noncentrality was an issue, this new solve would match reality
better, and the two solves would be closer to each other than the original poor
cross-validation
'''
import sys
import argparse
import re
import os
def parse_args():
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--mode',
choices=('too-close', 'too-far-from-center'),
default='too-close',
help='''What this tool does. "too-close": we throw out
some points that were too close to the camera.
"too-far-from-center": we throw out some points that
were observed too far from the imager center''')
parser.add_argument('--cull-percentile',
type=float,
default=10,
help='''The percentile of worst (too close, too far from
center of imager, ...) points to throw out''')
parser.add_argument('--gridn-width',
type = int,
default = 60,
help='''How densely we should sample the imager for the
uncertainty and cross-validation visualizations. Here we
just take the "width". The height will be automatically
computed based on the imager aspect ratio''')
parser.add_argument('--cbmax-diff',
type=float,
help='''The max-color to use for the diff plots. If
omitted, we use the default in
mrcal.show_projection_diff()''')
parser.add_argument('--cbmax-uncertainty',
type=float,
help='''The max-color to use for the uncertainty plots.
If omitted, we use the default in
mrcal.show_projection_uncertainty()''')
parser.add_argument('--hardcopy',
type=str,
help='''If given, we write the output plots to this
path. This path is given as DIR/FILE.EXTENSION. Multiple
plots will be made, to DIR/FILE-thing.EXTENSION''')
parser.add_argument('--terminal',
type=str,
help='''The gnuplot terminal to use for plots''')
parser.add_argument('models',
type=str,
nargs='+',
help='''The camera models to process. This requires an
even number of models, at least 2. If we are given
exactly two models, we use those two. If we are given
N>2 models, we join data from the first N/2 models and
the second N/2 models into two bigger sets of
calibration data, and we process those''')
args = parser.parse_args()
Nmodels = len(args.models)
if (Nmodels % 2):
print("We require an EVEN number of models", file=sys.stderr)
sys.exit(1)
return args
args = parse_args()
# I import the LOCAL mrcal
sys.path[:0] = f"{os.path.dirname(os.path.realpath(__file__))}/..",
import mrcal
import mrcal.model_analysis
import numpy as np
import numpysane as nps
import copy
import gnuplotlib as gp
def join_inputs(*optimization_inputs_all):
r'''Combines multiple calibration datasets into one
Intrinsics from the first input'''
if not all(o['intrinsics'].shape[-2] == 1 for o in optimization_inputs_all):
raise Exception('Everything must be MONOCULAR chessboard observations')
if not all(o.get('rt_cam_ref') is None or \
o['rt_cam_ref'].size == 0 \
for o in optimization_inputs_all):
raise Exception('Everything must be monocular chessboard observations with a STATIONARY camera')
if not all(o.get('points') is None or \
o['points'].size == 0 \
for o in optimization_inputs_all):
raise Exception('Everything must be monocular CHESSBOARD observations')
optimization_inputs = copy.deepcopy(optimization_inputs_all[0])
optimization_inputs['rt_ref_frame'] = \
nps.glue( *[ o['rt_ref_frame'] \
for o in optimization_inputs_all],
axis = -2 )
if not all( not np.any( o['indices_frame_camintrinsics_camextrinsics'][:,0] - np.arange(len(o['rt_ref_frame']))) \
for o in optimization_inputs_all ):
raise Exception("I assume frame indices starting at 0 and incrementing by 1")
Nobservations = \
sum(len(o['indices_frame_camintrinsics_camextrinsics']) \
for o in optimization_inputs_all)
optimization_inputs['indices_frame_camintrinsics_camextrinsics'] = \
np.zeros((Nobservations,3), dtype=np.int32)
optimization_inputs['indices_frame_camintrinsics_camextrinsics'][:,0] = \
np.arange(Nobservations, dtype=np.int32)
optimization_inputs['indices_frame_camintrinsics_camextrinsics'][:,2] = -1
optimization_inputs['observations_board'] = \
nps.glue( *[ o['observations_board'] \
for o in optimization_inputs_all],
axis = -4 )
optimization_inputs['imagepaths'] = \
nps.glue( *[ o['imagepaths'] \
for o in optimization_inputs_all],
axis = -1 )
return optimization_inputs
models = [mrcal.cameramodel(f) for f in args.models]
if len(models) > 2:
Nmodels = args.models
o0 = join_inputs( *[models[i].optimization_inputs() for i in range(0,Nmodels//2)] )
o1 = join_inputs( *[models[i].optimization_inputs() for i in range(Nmodels//2,Nmodels)] )
mrcal.optimize(**o0)
mrcal.optimize(**o1)
models = ( mrcal.cameramodel(optimization_inputs = o0,
icam_intrinsics = 0),
mrcal.cameramodel(optimization_inputs = o1,
icam_intrinsics = 0) )
percentile = args.cull_percentile
mode = args.mode
if mode == 'too-close':
what_culling = f'{percentile}% nearest'
what = 'range'
binwidth = 0.01
cull_nearest = True
elif mode == 'too-far-from-center':
what_culling = f'{percentile}% off-center'
what = 'pixel distance off-center'
binwidth = 20
cull_nearest = False
percentile = 100 - percentile
else:
# can't happen; checked above
raise
kwargs_show_uncertainty = dict()
if args.cbmax_uncertainty is not None:
kwargs_show_uncertainty['cbmax'] = args.cbmax_uncertainty
kwargs_show_diff = dict()
if args.cbmax_diff is not None:
kwargs_show_diff['cbmax'] = args.cbmax_diff
if args.hardcopy is None:
filename = None
else:
hardcopy_base,hardcopy_extension = os.path.splitext(args.hardcopy)
def reoptimize(imodel, model):
print('')
optimization_inputs = model.optimization_inputs()
observations_board = optimization_inputs['observations_board']
Noutliers = \
np.count_nonzero(observations_board[...,2] <= 0)
print(f"Before culling the {what_culling} points: {Noutliers=}")
if mode == 'too-close':
p = mrcal.hypothesis_board_corner_positions(**optimization_inputs)[0]
r = nps.mag(p)
elif mode == 'too-far-from-center':
if not mrcal.lensmodel_metadata_and_config(model.intrinsics()[0])['has_core']:
raise Exception("Here I'm assuming the model has an fxycxy core")
qcenter = model.intrinsics()[1][2:4]
r = nps.mag(observations_board[...,:2] - qcenter)
else:
# can't happen; checked above
raise
rthreshold = np.percentile(r.ravel(), percentile)
print(f"{what.capitalize()} at {percentile}-th percentile: {rthreshold:.2f}")
if False:
# This is a "directions" plot off residuals, with a
# range,qdiff_off_center domain. Hopefully I'll be able to see
# model-error patterns off this
x_board = mrcal.measurements_board(optimization_inputs)
p = mrcal.hypothesis_board_corner_positions(**optimization_inputs)[2]
r = nps.mag(p)
qcenter = model.intrinsics()[1][2:4]
idx_inliers = observations_board[...,2].ravel() > 0.
qobs_off_center = \
nps.clump(observations_board[...,:2], n=3)[idx_inliers] - \
qcenter
mag_qobs_off_center = nps.mag(qobs_off_center)
qobs_dir_off_center = np.array(qobs_off_center)
# to avoid /0
idx = mag_qobs_off_center>0
qobs_dir_off_center[idx] /= nps.dummy(mag_qobs_off_center[idx],
axis = -1)
x_board_radial_off_center = nps.inner(x_board, qobs_dir_off_center)
th = 180./np.pi * np.arctan2(x_board[...,1], x_board[...,0])
# hoping to see low-range points imply clustering in the residual
# direction
gp.plot( r,
mag_qobs_off_center,
th,
cbrange = [-180.,180.],
_with = 'points pt 7 palette',
_tuplesize = 3,
_set = 'palette defined ( 0 "#00ffff", 0.5 "#80ffff", 1 "#ffffff") model HSV')
# Hoping to see low ranges imply a non-zero bias on x_board_radial_off_center
gp.plot(r, x_board_radial_off_center, _with='points')
import IPython
IPython.embed()
sys.exit()
if args.hardcopy is not None:
filename = f"{hardcopy_base}-histogram-measurements-cull-camera{imodel}{hardcopy_extension}"
else:
filename = None
histogram = gp.gnuplotlib()
histogram.plot(r.ravel(),
histogram = True,
binwidth = binwidth,
_set = f'arrow from {rthreshold},graph 0 to {rthreshold},graph 1 nohead front',
title = f'Histogram of {what}, with the {what_culling} points marked: camera {imodel}',
hardcopy = filename,
terminal = args.terminal)
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
if cull_nearest:
i_cull = r.ravel() < rthreshold
else:
i_cull = r.ravel() > rthreshold
nps.clump(observations_board, n=3)[i_cull, 2] = -1
Noutliers = \
np.count_nonzero(observations_board[...,2] <= 0)
print(f"After culling the {what_culling} points: {Noutliers=}")
mrcal.optimize(**optimization_inputs)
return (mrcal.cameramodel(optimization_inputs = optimization_inputs,
icam_intrinsics = 0),
histogram)
plots = []
models_plots_reoptimized = [ reoptimize(i,m) for i,m in enumerate(models) ]
models_reoptimized = [ m for (m,p) in models_plots_reoptimized ]
plots.extend([ p for (m,p) in models_plots_reoptimized ])
for i,m in enumerate(models_reoptimized):
if args.hardcopy is not None:
filename = f"{hardcopy_base}-uncertainty-post-cull-camera{i}{hardcopy_extension}"
plots.append( \
mrcal.show_projection_uncertainty(
m,
gridn_width = args.gridn_width,
title = f'Uncertainty after cutting the {what_culling} points: camera {i}',
hardcopy = filename,
terminal = args.terminal,
**kwargs_show_uncertainty) )
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
for i in range(len(models)):
if args.hardcopy is not None:
filename = f"{hardcopy_base}-diff-from-cull-camera{i}{hardcopy_extension}"
plots.append( \
mrcal.show_projection_diff( \
(models[i],models_reoptimized[i]),
gridn_width = args.gridn_width,
use_uncertainties = False,
focus_radius = 100,
title = f'Reoptimizing after cutting the {what_culling} points: resulting diff for camera {i}',
hardcopy = filename,
terminal = args.terminal,
**kwargs_show_diff)[0] )
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
if len(models) != 2:
print("WARNING: validate_noncentral() is intended to work with exactly two models. Got something different; not showing the new cross-validation diff")
else:
if args.hardcopy is not None:
filename = f"{hardcopy_base}-cross-validation-pre-cull-camera{i}{hardcopy_extension}"
plots.append( \
mrcal.show_projection_diff((models[0],models[1]),
gridn_width = args.gridn_width,
use_uncertainties = False,
focus_radius = 100,
title = f'Original, poor cross-validation diff',
hardcopy = filename,
terminal = args.terminal,
**kwargs_show_diff)[0])
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
if args.hardcopy is not None:
filename = f"{hardcopy_base}-cross-validation-post-cull-camera{i}{hardcopy_extension}"
plots.append( \
mrcal.show_projection_diff((models_reoptimized[0],models_reoptimized[1]),
gridn_width = args.gridn_width,
use_uncertainties = False,
focus_radius = 100,
title = f'Cross-validation diff after cutting the {what_culling} points',
hardcopy = filename,
terminal = args.terminal,
**kwargs_show_diff)[0])
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
# Needs gnuplotlib >= 0.42
gp.wait(*plots)
mrcal-2.5/analyses/validate-uncertainty.py 0000775 0000000 0000000 00000024727 15123677724 0021040 0 ustar 00root root 0000000 0000000 #!/usr/bin/env python3
# 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
r'''Study the uncertainty as a predictor of cross-validation diffs
SYNOPSIS
$ validate-uncertainty.py camera.cameramodel
... plots pop up, showing the uncertainty prediction from the given models,
... and cross-validation diff obtained from creating perfect data corrupted
... ONLY with perfect gaussian noise. If the noise on the inputs was the ONLY
... source of error (what the uncertainty modeling expects), then the
... uncertainty plots would predict the cross-validation plots well
A big feature of mrcal is the ability to gauge the accuracy of the solved
intrinsics: by computing the projection uncertainty. This measures the
sensitivity of the solution to noise in the inputs. So using this as a measure
of calibration accuracy makes a core assumption: this input noise is the only
source of error. This assumption is often false, so cross-validation diffs can
be computed to sample the full set of error sources, not just this one.
Sometimes we see cross-validation results higher than what the uncertainties
promise us, and figuring out the reason can be challenging. This tool serves to
validate the techniques to help in that debugging.
'''
import sys
import argparse
import re
import os
def parse_args():
parser = \
argparse.ArgumentParser(description = __doc__,
formatter_class=argparse.RawDescriptionHelpFormatter)
parser.add_argument('--num-samples',
type = int,
default = 16,
help='''How many noisy-data samples to process''')
parser.add_argument('--observed-pixel-uncertainty',
type = float,
help='''How much noise to inject into perfect solves. If
omitted, we infer this noise level from the model''')
parser.add_argument('--gridn-width',
type = int,
default = 60,
help='''How densely we should sample the imager for the
uncertainty and cross-validation visualizations. Here we
just take the "width". The height will be automatically
computed based on the imager aspect ratio''')
parser.add_argument('--cbmax-diff',
type=float,
help='''The max-color to use for the diff plots. If
omitted, we use the default in
mrcal.show_projection_diff()''')
parser.add_argument('--cbmax-uncertainty',
type=float,
help='''The max-color to use for the uncertainty plots.
If omitted, we use the default in
mrcal.show_projection_uncertainty()''')
parser.add_argument('--hardcopy',
type=str,
help='''If given, we write the output plots to this
path. This path is given as DIR/FILE.EXTENSION. Multiple
plots will be made, to DIR/FILE-thing.EXTENSION''')
parser.add_argument('--terminal',
type=str,
help='''The gnuplot terminal to use for plots''')
parser.add_argument('model',
type=str,
help='''The camera model to process''')
return parser.parse_args()
args = parse_args()
# I import the LOCAL mrcal
sys.path[:0] = f"{os.path.dirname(os.path.realpath(__file__))}/..",
import mrcal
import mrcal.model_analysis
import numpy as np
import numpysane as nps
import gnuplotlib as gp
import copy
# Today I hardcode this. I'm mostly thinking of monocular chessboard solves
# only. Presumably other behave the same way
icam_intrinsics = 0
if args.hardcopy is None:
filename = None
else:
hardcopy_base,hardcopy_extension = os.path.splitext(args.hardcopy)
def apply_noise(optimization_inputs,
*,
observed_pixel_uncertainty):
noise_nominal = \
observed_pixel_uncertainty * \
np.random.randn(*optimization_inputs['observations_board'][...,:2].shape)
weight = nps.dummy( optimization_inputs['observations_board'][...,2],
axis = -1 )
weight[ weight<=0 ] = 1. # to avoid dividing by 0
optimization_inputs['observations_board'][...,:2] += \
noise_nominal / weight
kwargs_show_uncertainty = dict()
if args.cbmax_uncertainty is not None:
kwargs_show_uncertainty['cbmax'] = args.cbmax_uncertainty
kwargs_show_diff = dict()
if args.cbmax_diff is not None:
kwargs_show_diff['cbmax'] = args.cbmax_diff
model = mrcal.cameramodel(args.model)
optimization_inputs = model.optimization_inputs()
if args.observed_pixel_uncertainty is not None:
observed_pixel_uncertainty = args.observed_pixel_uncertainty
else:
observed_pixel_uncertainty = mrcal.model_analysis._observed_pixel_uncertainty_from_inputs(optimization_inputs)
print(f"Inferred {observed_pixel_uncertainty=:.2f}")
plots = []
if args.hardcopy is not None:
filename = f"{hardcopy_base}-uncertainty-baseline{hardcopy_extension}"
plots.append( \
mrcal.show_projection_uncertainty(model,
gridn_width = args.gridn_width,
observed_pixel_uncertainty = observed_pixel_uncertainty,
title = f'Baseline uncertainty with {observed_pixel_uncertainty=}',
hardcopy = filename,
terminal = args.terminal,
**kwargs_show_uncertainty) )
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
optimization_inputs_perfect = optimization_inputs
mrcal.make_perfect_observations(optimization_inputs_perfect,
observed_pixel_uncertainty = 0)
def model_sample():
optimization_inputs = copy.deepcopy(optimization_inputs_perfect)
apply_noise(optimization_inputs,
observed_pixel_uncertainty = observed_pixel_uncertainty)
mrcal.optimize(**optimization_inputs)
return mrcal.cameramodel(optimization_inputs = optimization_inputs,
icam_intrinsics = icam_intrinsics)
model0 = model_sample()
models1 = [model_sample() for _ in range(args.num_samples)]
if args.hardcopy is not None:
filename = f"{hardcopy_base}-uncertainty-perfect{hardcopy_extension}"
plots.append( \
mrcal.show_projection_uncertainty(model0,
gridn_width = args.gridn_width,
observed_pixel_uncertainty = observed_pixel_uncertainty,
title = 'Uncertainty with perfect observations + noise; should be very close to baseline',
hardcopy = filename,
terminal = args.terminal,
**kwargs_show_uncertainty) )
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
def gnuplotlib_normalize_options_dict(d):
d2 = {}
for key in d:
gp.add_plot_option(d2, key, d[key])
return d2
gridn_plot = int( np.ceil(np.sqrt(len(models1)) ) )
diff_multiplot_args = []
for model1 in models1:
data_tuples,plot_options = \
mrcal.show_projection_diff((model0,model1),
gridn_width = args.gridn_width,
use_uncertainties = False,
focus_radius = np.min(model.imagersize())//8,
title = '',
unset=('key',
'xtics',
'ytics',),
contour_labels_styles = None, # no label
_set=('lmargin 0',
'tmargin 0',
'rmargin 0',
'bmargin 0',
),
return_plot_args = True,
**kwargs_show_diff)[0]
plot_options = gnuplotlib_normalize_options_dict(plot_options)
subplot_options = dict()
for o in plot_options:
if o in gp.knownSubplotOptions:
subplot_options[o] = plot_options[o]
diff_multiplot_args.append( (*data_tuples,subplot_options) )
# massage one of the plot_options from the subplots; they're probably all the same
_plot_options = copy.copy(plot_options)
plot_options = dict()
for o in _plot_options:
if o not in gp.knownSubplotOptions:
plot_options[o] = _plot_options[o]
if args.hardcopy is not None:
filename = f"{hardcopy_base}-diff-samples{hardcopy_extension}"
diff_multiplot = gp.gnuplotlib( title = 'Simulated cross-validation diff samples comparing two perfectly-noised models',
multiplot = f'layout {gridn_plot},{gridn_plot}',
hardcopy = filename,
terminal = args.terminal,
**plot_options )
diff_multiplot.plot( *diff_multiplot_args )
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
plots.append(diff_multiplot)
if args.hardcopy is not None:
filename = f"{hardcopy_base}-diff-joint-stdev{hardcopy_extension}"
plots.append( mrcal.show_projection_diff([model0, *models1],
gridn_width = args.gridn_width,
use_uncertainties = False,
focus_radius = np.min(model.imagersize())//8,
title = 'Simulated cross-validation diff: stdev of of ALL the samples',
hardcopy = filename,
terminal = args.terminal,
**kwargs_show_diff)[0])
if args.hardcopy is not None:
print(f"Wrote '{filename}'")
if args.hardcopy is None:
# Needs gnuplotlib >= 0.42
gp.wait(*plots)
mrcal-2.5/basic-geometry.h 0000664 0000000 0000000 00000012674 15123677724 0015571 0 ustar 00root root 0000000 0000000 // 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
#ifdef __cplusplus
extern "C" {
#endif
// Old g++ doesn't work for some of this. Let's say I have this:
//
// typedef union
// {
// struct
// {
// double x,y;
// };
// double xy[2];
// } mrcal_point2_t;
// 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 };
// }
//
// This happens on g++ <= 10:
//
// dima@fatty:/tmp$ g++-10 -c -o tst.o tst.c
// tst.c: In function ‘mrcal_point2_t mrcal_point2_sub(mrcal_point2_t, mrcal_point2_t)’:
// tst.c:14:45: error: too many initializers for ‘mrcal_point2_t’
// 14 | .y = a.y - b.y };
// | ^
//
// Since this happens only with c++ and with old compilers, I don't give the
// unhappy code to the old compiler. People stuck in that world will need to
// make do without these functions. g++ < 11 is affected clang++ >= 6 seems ok,
// so I just accept all clang
#if defined __cplusplus && (__GNUC__ < 11 && !(defined __clang__ && __clang__))
#define MRCAL_NO_CPP_ANONYMOUS_NAMED_INITIALIZERS 1
#endif
// 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;
#ifndef MRCAL_NO_CPP_ANONYMOUS_NAMED_INITIALIZERS
//////////// Easy convenience stuff
////// point2
__attribute__((unused))
static double mrcal_point2_inner(const mrcal_point2_t a, const mrcal_point2_t b)
{
return
a.x*b.x +
a.y*b.y;
}
__attribute__((unused))
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
__attribute__((unused))
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 };
}
__attribute__((unused))
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 };
}
__attribute__((unused))
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
__attribute__((unused))
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;
}
__attribute__((unused))
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
__attribute__((unused))
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 };
}
__attribute__((unused))
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 };
}
__attribute__((unused))
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 };
}
__attribute__((unused))
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 };
}
#endif // MRCAL_NO_CPP_ANONYMOUS_NAMED_INITIALIZERS
#define mrcal_point2_print(p) do { printf( #p " = (%f %f)\n", (p).x, (p).y); } while(0)
#define mrcal_point3_print(p) do { printf( #p " = (%f %f %f)\n", (p).x, (p).y, (p).z); } while(0)
#define mrcal_Rt_print(Rt) do { printf( #Rt " = (\n %f %f %f\n %f %f %f\n %f %f %f\n ============\n %f %f %f\n)\n", \
(Rt)[0],(Rt)[1],(Rt)[2],(Rt)[3],(Rt)[4],(Rt)[5],(Rt)[6],(Rt)[7],(Rt)[8],(Rt)[9],(Rt)[10],(Rt)[11]); } while(0)
#define mrcal_rt_print(rt) do { printf( #rt " = (%f %f %f; %f %f %f\n", \
(rt)[0],(rt)[1],(rt)[2],(rt)[3],(rt)[4],(rt)[5]); } while(0)
#ifdef __cplusplus
}
#endif
mrcal-2.5/cahvore.cc 0000664 0000000 0000000 00000026725 15123677724 0014446 0 ustar 00root root 0000000 0000000 // Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
// Government sponsorship acknowledged. All rights reserved.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
#include
#include
#include "_autodiff.hh"
extern "C" {
#include "_cahvore.h"
}
template
static
bool _project_cahvore_internals( // outputs
vec_withgrad_t* pdistorted,
// inputs
const vec_withgrad_t& p,
const val_withgrad_t& alpha,
const val_withgrad_t& beta,
const val_withgrad_t& r0,
const val_withgrad_t& r1,
const val_withgrad_t& r2,
const val_withgrad_t& e0,
const val_withgrad_t& e1,
const val_withgrad_t& e2,
const double cahvore_linearity)
{
// Apply a CAHVORE warp to an un-distorted point
// Given intrinsic parameters of a CAHVORE model and a set of
// camera-coordinate points, return the projected point(s)
// This comes from cmod_cahvore_3d_to_2d_general() in
// m-jplv/libcmod/cmod_cahvore.c
//
// The lack of documentation here comes directly from the lack of
// documentation in that function.
// I parametrize the optical axis such that
// - o(alpha=0, beta=0) = (0,0,1) i.e. the optical axis is at the center
// if both parameters are 0
// - The gradients are cartesian. I.e. do/dalpha and do/dbeta are both
// NOT 0 at (alpha=0,beta=0). This would happen at the poles (gimbal
// lock), and that would make my solver unhappy
// So o = { s_al*c_be, s_be, c_al*c_be }
vec_withgrad_t sca = alpha.sincos();
vec_withgrad_t scb = beta .sincos();
vec_withgrad_t o;
o[0] = scb[1]*sca[0];
o[1] = scb[0];
o[2] = scb[1]*sca[1];
// Note: CAHVORE is noncentral: project(p) and project(k*p) do NOT
// project to the same point
// What is called "omega" in the canonical CAHVOR implementation is
// called "zeta" in the canonical CAHVORE implementation. They're the
// same thing
// cos( angle between p and o ) = inner(p,o) / (norm(o) * norm(p)) =
// zeta/norm(p)
val_withgrad_t zeta = p.dot(o);
// Basic Computations
// Calculate initial terms
vec_withgrad_t u = o*zeta;
vec_withgrad_t ll = p - u;
val_withgrad_t l = ll.mag();
// Calculate theta using Newton's Method
val_withgrad_t theta = l.atan2(zeta);
int inewton;
for( inewton = 100; inewton; inewton--)
{
// Compute terms from the current value of theta
vec_withgrad_t scth = theta.sincos();
val_withgrad_t theta2 = theta * theta;
val_withgrad_t theta3 = theta * theta2;
val_withgrad_t theta4 = theta * theta3;
val_withgrad_t upsilon =
zeta*scth[1] + l*scth[0]
+ (scth[1] - 1.0 ) * (e0 + e1*theta2 + e2*theta4)
- (theta - scth[0]) * ( e1*theta*2.0 + e2*theta3*4.0);
// Update theta
val_withgrad_t dtheta =
(zeta*scth[0] - l*scth[1]
- (theta - scth[0]) * (e0 + e1*theta2 + e2*theta4)) / upsilon;
theta -= dtheta;
// Check exit criterion from last update
if(fabs(dtheta.x) < 1e-8)
break;
}
if(inewton == 0)
{
fprintf(stderr, "%s(): too many iterations\n", __func__);
return false;
}
// got a theta
// Check the value of theta
if(theta.x * fabs(cahvore_linearity) > M_PI/2.)
{
fprintf(stderr, "%s(): theta out of bounds\n", __func__);
return false;
}
// If we aren't close enough to use the small-angle approximation ...
if (theta.x > 1e-8)
{
// ... do more math!
val_withgrad_t linth = theta * cahvore_linearity;
val_withgrad_t chi;
if (cahvore_linearity < -1e-15)
chi = linth.sin() / cahvore_linearity;
else if (cahvore_linearity > 1e-15)
chi = linth.tan() / cahvore_linearity;
else
chi = theta;
val_withgrad_t chi2 = chi * chi;
val_withgrad_t chi3 = chi * chi2;
val_withgrad_t chi4 = chi * chi3;
val_withgrad_t zetap = l / chi;
val_withgrad_t mu = r0 + r1*chi2 + r2*chi4;
vec_withgrad_t uu = o*zetap;
vec_withgrad_t vv = ll * (mu + 1.);
*pdistorted = uu + vv;
}
else
*pdistorted = p;
return true;
}
// Not meant to be touched by the end user. Implemented separate from mrcal.c so
// that I can get automated gradient propagation with c++
extern "C"
__attribute__ ((visibility ("hidden")))
bool project_cahvore_internals( // outputs
mrcal_point3_t* __restrict pdistorted,
double* __restrict dpdistorted_dintrinsics_nocore,
double* __restrict dpdistorted_dp,
// inputs
const mrcal_point3_t* __restrict p,
const double* __restrict intrinsics_nocore,
const double cahvore_linearity)
{
if( dpdistorted_dintrinsics_nocore == NULL &&
dpdistorted_dp == NULL )
{
// No gradients. NGRAD in all the templates is 0
vec_withgrad_t<0,3> pdistortedg;
vec_withgrad_t<0,8> ig;
ig.init_vars(intrinsics_nocore,
0,8, // ivar0,Nvars
-1 // i_gradvec0
);
vec_withgrad_t<0,3> pg;
pg.init_vars(p->xyz,
0,3, // ivar0,Nvars
-1 // i_gradvec0
);
if(!_project_cahvore_internals(&pdistortedg,
pg,
ig[0],
ig[1],
ig[2],
ig[3],
ig[4],
ig[5],
ig[6],
ig[7],
cahvore_linearity))
return false;
pdistortedg.extract_value(pdistorted->xyz);
return true;
}
if( dpdistorted_dintrinsics_nocore == NULL &&
dpdistorted_dp != NULL )
{
// 3 variables: p (3 vars)
vec_withgrad_t<3,3> pdistortedg;
vec_withgrad_t<3,8> ig;
ig.init_vars(intrinsics_nocore,
0,8, // ivar0,Nvars
-1 // i_gradvec0
);
vec_withgrad_t<3,3> pg;
pg.init_vars(p->xyz,
0,3, // ivar0,Nvars
0 // i_gradvec0
);
if(!_project_cahvore_internals(&pdistortedg,
pg,
ig[0],
ig[1],
ig[2],
ig[3],
ig[4],
ig[5],
ig[6],
ig[7],
cahvore_linearity))
return false;
pdistortedg.extract_value(pdistorted->xyz);
pdistortedg.extract_grad (dpdistorted_dp,
0,3, // ivar0,Nvars
0, // i_gradvec0
sizeof(double)*3, sizeof(double),
3 // Nvars
);
return true;
}
if( dpdistorted_dintrinsics_nocore != NULL &&
dpdistorted_dp == NULL )
{
// 8 variables: alpha,beta,R,E (8 vars)
vec_withgrad_t<8,3> pdistortedg;
vec_withgrad_t<8,8> ig;
ig.init_vars(intrinsics_nocore,
0,8, // ivar0,Nvars
0 // i_gradvec0
);
vec_withgrad_t<8,3> pg;
pg.init_vars(p->xyz,
0,3, // ivar0,Nvars
-1 // i_gradvec0
);
if(!_project_cahvore_internals(&pdistortedg,
pg,
ig[0],
ig[1],
ig[2],
ig[3],
ig[4],
ig[5],
ig[6],
ig[7],
cahvore_linearity))
return false;
pdistortedg.extract_value(pdistorted->xyz);
pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore,
0,8, // i_gradvec0,N_gradout
0, // ivar0
sizeof(double)*8, sizeof(double),
3 // Nvars
);
return true;
}
if( dpdistorted_dintrinsics_nocore != NULL &&
dpdistorted_dp != NULL )
{
// 11 variables: alpha,beta,R,E (8 vars) + p (3 vars)
vec_withgrad_t<11,3> pdistortedg;
vec_withgrad_t<11,8> ig;
ig.init_vars(intrinsics_nocore,
0,8, // ivar0,Nvars
0 // i_gradvec0
);
vec_withgrad_t<11,3> pg;
pg.init_vars(p->xyz,
0,3, // ivar0,Nvars
8 // i_gradvec0
);
if(!_project_cahvore_internals(&pdistortedg,
pg,
ig[0],
ig[1],
ig[2],
ig[3],
ig[4],
ig[5],
ig[6],
ig[7],
cahvore_linearity))
return false;
pdistortedg.extract_value(pdistorted->xyz);
pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore,
0,8, // i_gradvec0,N_gradout
0, // ivar0
sizeof(double)*8, sizeof(double),
3 // Nvars
);
pdistortedg.extract_grad (dpdistorted_dp,
8,3, // ivar0,Nvars
0, // i_gradvec0
sizeof(double)*3, sizeof(double),
3 // Nvars
);
return true;
}
fprintf(stderr, "Getting here is a bug. Please report\n");
assert(0);
}
mrcal-2.5/cameramodel-parser.re 0000664 0000000 0000000 00000060003 15123677724 0016566 0 ustar 00root root 0000000 0000000 /* -*- 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
// Apparently I need this in MSVC to get constants
#define _USE_MATH_DEFINES
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include
#include "mrcal.h"
#include "_util.h"
#define DEBUG 0
// string defined by an explicit length. Instead of being 0-terminated
typedef struct
{
const char* s;
int len;
} string_t;
/*!re2c
re2c:define:YYCTYPE = char;
// No filling. All the data is available at the start
re2c:yyfill:enable = 0;
// I use @ tags
re2c:flags:tags = 1;
re2c:sentinel = 0;
SPACE = [ \t\n\r]*;
IGNORE = (SPACE | "#" [^\x00\n]* "\n")*;
// This FLOAT definition will erroneously accept "." and "" as a valid float,
// but the strtod converter will then reject it
FLOAT = "-"?[0-9]*("."[0-9]*)?([eE][+-]?[0-9]+)?;
UNSIGNED_INT = "0"|[1-9][0-9]*;
*/
/*!stags:re2c format = 'static const char *@@;'; */
static bool string_is(const char* ref, string_t key)
{
int ref_strlen = strlen(ref);
return
ref_strlen == key.len &&
0 == strncmp(key.s, ref, ref_strlen);
}
static bool read_string( // output stored here. If NULL, we try to read off
// a string, but do not store it; success in the
// return value, as usual
string_t* out,
const char** pYYCURSOR,
const char* start_file,
const char* what)
{
const char* YYMARKER;
const char* YYCURSOR = *pYYCURSOR;
while(true)
{
const char* s;
const char* e;
/*!re2c
IGNORE "b"? ["'] @s [^"'\x00]* @e ["']
{
if(out != NULL)
{
out->s = s;
out->len = (int)(e-s);
}
*pYYCURSOR = YYCURSOR;
return true;
}
* { break; }
*/
}
if(out != NULL)
MSG("Didn't see the %s string at position %ld. Giving up.",
what, (long int)(*pYYCURSOR - start_file));
return false;
}
static bool read_value( const char** pYYCURSOR,
const char* start_file)
{
const char* YYMARKER;
const char* YYCURSOR = *pYYCURSOR;
const char* s, *e;
while(true)
{
/*!re2c
IGNORE @s FLOAT @e
{
// FLOAT regex erroneously matches empty strings and ".". I
// explicitly check for those, and return a failure
if( e == s )
return false;
if( e == &s[1] && *s == '.')
return false;
break;
}
*
{
return false;
}
*/
}
*pYYCURSOR = YYCURSOR;
return true;
}
typedef bool (ingest_generic_consume_ignorable_t)(void* out0, int i,
const char** pYYCURSOR,
const char* start_file,
const char* what);
static bool ingest_double_consume_ignorable(void* out0, int i,
const char** pYYCURSOR,
const char* start_file,
const char* what)
{
const char* YYMARKER;
const char* YYCURSOR = *pYYCURSOR;
const char* s;
const char* e;
while(true)
{
/*!re2c
IGNORE @s FLOAT @e IGNORE
{
break;
}
*
{
MSG("Error parsing double-precision value for %s at %ld",
what, (long int)(*pYYCURSOR-start_file));
return false;
}
*/
}
if(out0 != NULL)
{
int N = e-s;
char tok[N+1];
memcpy(tok, s, N);
tok[N] = '\0';
char* endptr = NULL;
((double*)out0)[i] = strtod(tok, &endptr);
if( N == 0 || endptr == NULL || endptr != &tok[N] ||
!isfinite(((double*)out0)[i]))
{
MSG("Error parsing double-precision value for %s at %ld. String: '%s'",
what, (long int)(*pYYCURSOR-start_file), tok);
return false;
}
}
*pYYCURSOR = YYCURSOR;
return true;
}
static bool ingest_uint_consume_ignorable(void* out0, int i,
const char** pYYCURSOR,
const char* start_file,
const char* what)
{
const char* YYMARKER;
const char* YYCURSOR = *pYYCURSOR;
const char* s;
const char* e;
while(true)
{
/*!re2c
IGNORE @s UNSIGNED_INT @e IGNORE
{
break;
}
*
{
MSG("Error parsing unsigned integer for %s at %ld",
what, (long int)(*pYYCURSOR-start_file));
return false;
}
*/
}
if(out0 != NULL)
{
int N = e-s;
char tok[N+1];
memcpy(tok, s, N);
tok[N] = '\0';
int si = atoi(tok);
if( N == 0 || si < 0 )
{
MSG("Error parsing unsigned int for %s at %ld. String: '%s'",
what, (long int)(*pYYCURSOR-start_file), tok);
return false;
}
((unsigned int*)out0)[i] = (unsigned int)si;
}
*pYYCURSOR = YYCURSOR;
return true;
}
static bool read_list_values_generic( // output stored here. If NULL, we try to
// read off the values, but do not store
// them; success in the return value, as
// usual
void* out,
ingest_generic_consume_ignorable_t* f,
const char** pYYCURSOR, const char* start_file,
const char* what,
int Nvalues)
{
const char* YYMARKER;
const char* YYCURSOR = *pYYCURSOR;
while(true)
{
/*!re2c
IGNORE [[(] { break; }
*
{
MSG("Didn't see the opening [/( for the %s at position %ld. Giving up.",
what, (long int)(YYCURSOR - start_file));
return false;
}
*/
}
int i;
for(i=0; i 0)
{
// closed inner ]. Trailing , afterwards is optional. I don't
// bother checking for this thoroughly, so I end up erroneously
// accepting expressions like [1,2,3][3,4,5]. But that's OK
;
}
continue;
}
*
{
MSG("Error reading a balanced list at %ld (unexpected value)",
(long int)(YYCURSOR-start_file));
return false;
}
*/
}
MSG("Getting here is a bug");
return false;
}
// Internal routine that does all the work
static
bool read_cameramodel_from_string(// output buffer. If it should be allocated,
// *model == NULL at the start. Otherwise
// *model is the preallocated buffer
mrcal_cameramodel_VOID_t** model,
// if the buffer is preallocated
// (*model!=NULL), the number of the
// intrinsics available in this buffer is in
// *Nintrinsics_max. If this is insufficient,
// we fail, set the number of intrinsics
// needed in *Nintrinsics_max, and return
// false. If we fail for any other reason, we
// set *Nintrinsics_max=0. If the buffer
// should be allocated, this isn't used
int* Nintrinsics_max,
// in
// 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
const char* string,
const int len)
{
bool model_need_dealloc = false;
bool did_read_intrinsics = false;
bool did_set_Nintrinsics_max = false;
// Set the output structure to invalid values that I can check later
// Everything except the intrinsics will be read here, and moved to *model
// at the end. The intrinsics are read directly into *model. This allows us
// to read in non-intrinsics before the full array is allocated
mrcal_cameramodel_VOID_t model_not_intrinsics =
{.rt_cam_ref[0] = DBL_MAX,
.imagersize = {},
.lensmodel.type = MRCAL_LENSMODEL_INVALID };
bool finished = false;
const char* YYMARKER;
const char* start_file = NULL;
// This is lame. If the end of the buffer is indicated by the buffer length
// only, I allocate a new padded buffer, and copy into it. Then this code
// looks for a terminated 0 always. I should instead use the re2c logic for
// fixed-length buffers (YYLIMIT), but that looks complicated
const char* YYCURSOR = NULL;
char* malloced_buf = NULL;
if(len > 0)
{
malloced_buf = malloc(len+1);
if(malloced_buf == NULL)
{
MSG("malloc() failed");
goto done;
}
memcpy(malloced_buf, string, len);
malloced_buf[len] = '\0';
YYCURSOR = malloced_buf;
}
else
YYCURSOR = string;
start_file = YYCURSOR;
///////// leading {
while(true)
{
/*!re2c
IGNORE "{" IGNORE { break; }
*
{
MSG("Didn't see leading '{'. Giving up.");
goto done;
}
*/
}
///////// key: value
while(true)
{
string_t key = {};
///////// key:
if(!read_string(&key, &YYCURSOR, start_file, "key"))
goto done;
while(true)
{
/*!re2c
IGNORE ":" { break; }
*
{
MSG("Didn't see expected ':' at %ld. Giving up.",
(long int)(YYCURSOR-start_file));
goto done;
}
*/
}
#if defined DEBUG && DEBUG
MSG("key: '%.*s'", key.len, key.s);
#endif
if( string_is("lensmodel", key) )
{
if(model_not_intrinsics.lensmodel.type >= 0)
{
MSG("lensmodel defined more than once");
goto done;
}
// "lensmodel" has string values
string_t lensmodel;
if(!read_string(&lensmodel,
&YYCURSOR, start_file, "lensmodel"))
goto done;
char lensmodel_string[lensmodel.len+1];
memcpy(lensmodel_string, lensmodel.s, lensmodel.len);
lensmodel_string[lensmodel.len] = '\0';
if( !mrcal_lensmodel_from_name(&model_not_intrinsics.lensmodel, lensmodel_string) )
{
MSG("Could not parse lensmodel '%s'", lensmodel_string);
goto done;
}
}
else if(string_is("intrinsics", key))
{
if(did_read_intrinsics)
{
MSG("intrinsics defined more than once");
goto done;
}
if(model_not_intrinsics.lensmodel.type < 0)
{
MSG("Saw 'intrinsics' key, before a 'lensmodel' key. Make sure that a 'lensmodel' key exists, and that it appears in the file before the 'intrinsics'");
goto done;
}
const int Nintrinsics = mrcal_lensmodel_num_params(&model_not_intrinsics.lensmodel);
if(*model == NULL)
{
// we need to allocate the model
*model = malloc(sizeof(mrcal_cameramodel_VOID_t) +
Nintrinsics*sizeof(double));
if(NULL == *model)
{
MSG("malloc() failed");
goto done;
}
model_need_dealloc = true;
}
else
{
// we read the data into a static buffer
if(Nintrinsics > *Nintrinsics_max)
{
*Nintrinsics_max = Nintrinsics;
did_set_Nintrinsics_max = true;
goto done;
}
}
if( !read_list_values_generic((*model)->intrinsics,
ingest_double_consume_ignorable,
&YYCURSOR, start_file,
"intrinsics", Nintrinsics) )
goto done;
did_read_intrinsics = true;
}
else if(string_is("extrinsics", key) ||
string_is("rt_cam_ref", key))
{
if(model_not_intrinsics.rt_cam_ref[0] != DBL_MAX)
{
// Receiving another one; must be identical
double rt_cam_ref[6];
if( !read_list_values_generic(rt_cam_ref,
ingest_double_consume_ignorable,
&YYCURSOR, start_file, "extrinsics", 6) )
goto done;
for(int i=0; i<6; i++)
if(1e-9 < fabs(rt_cam_ref[i] - model_not_intrinsics.rt_cam_ref[i]))
{
MSG("extrinsics defined more than once and aren't identical");
goto done;
}
}
else if( !read_list_values_generic(model_not_intrinsics.rt_cam_ref,
ingest_double_consume_ignorable,
&YYCURSOR, start_file, "extrinsics", 6) )
goto done;
}
else if(string_is("imagersize", key))
{
if(model_not_intrinsics.imagersize[0] > 0)
{
MSG("imagersize defined more than once");
goto done;
}
if( !read_list_values_generic(model_not_intrinsics.imagersize,
ingest_uint_consume_ignorable,
&YYCURSOR, start_file, "imagersize", 2) )
goto done;
}
else
{
// Some unknown key. Read off the data and continue
// try to read a string...
if(!read_value(&YYCURSOR, start_file) &&
!read_string(NULL, &YYCURSOR, start_file, "unknown") &&
!read_balanced_list(&YYCURSOR, start_file))
{
MSG("Error parsing value for key '%.*s' at %ld",
key.len, key.s,
(long int)(YYCURSOR-start_file));
goto done;
}
}
// Done with key:value. Look for optional trailing , and/or a }. We must
// see at least some of those things. k:v k:v without a , in-between is
// illegal
bool closing_brace = false;
const char* f;
while(true)
{
/*!re2c
IGNORE ("," | "}" | ("," IGNORE "}")) @f
{
if(f[-1] == '}') closing_brace = true;
break;
}
*
{
MSG("Didn't see trailing , at %ld",
(long int)(YYCURSOR-start_file));
goto done;
}
*/
}
if(closing_brace)
{
while(true)
{
/*!re2c
IGNORE [\x00]
{
finished = true;
goto done;
}
*
{
MSG("Garbage after trailing } at %ld. Giving up",
(long int)(f-1 - start_file));
goto done;
}
*/
}
}
}
done:
if(Nintrinsics_max != NULL && !did_set_Nintrinsics_max)
*Nintrinsics_max = 0;
free(malloced_buf);
if(!finished)
{
if(model_need_dealloc)
{
free(*model);
*model = NULL;
}
return false;
}
// Done parsing everything! Validate and finalize
if(!(model_not_intrinsics.lensmodel.type >= 0 &&
did_read_intrinsics &&
model_not_intrinsics.rt_cam_ref[0] != DBL_MAX &&
model_not_intrinsics.imagersize[0] > 0))
{
MSG("Incomplete cameramodel. Need keys: lensmodel, intrinsics, rt_cam_ref/extrinsics, imagersize");
if(model_need_dealloc)
{
free(*model);
*model = NULL;
}
return false;
}
memcpy(*model, &model_not_intrinsics, sizeof(model_not_intrinsics));
return true;
}
static
bool read_cameramodel_from_file(// output buffer. If it should be allocated,
// *model == NULL at the start. Otherwise *model
// is the preallocated buffer
mrcal_cameramodel_VOID_t** model,
// if the buffer is preallocated (*model!=NULL),
// the number of the intrinsics available in
// this buffer is in *Nintrinsics_max. If this
// is insufficient, we fail, set the number of
// intrinsics needed in *Nintrinsics_max, and
// return false. If we fail for any other
// reason, we set *Nintrinsics_max=0. If the
// buffer should be allocated, this isn't used
int* Nintrinsics_max,
// in
const char* filename)
{
int fd = -1;
char* string = NULL;
bool result = false;
bool did_set_Nintrinsics_max = false;
fd = open(filename, O_RDONLY);
if(fd < 0)
{
MSG("Couldn't open(\"%s\")", filename);
goto done;
}
struct stat st;
if(0 != fstat(fd, &st))
{
MSG("Couldn't stat(\"%s\")", filename);
goto done;
}
// I mmap twice:
//
// 1. anonymous mapping slightly larger than the file size. These are all 0
// 2. mmap of the file. The trailing 0 are preserved, and the parser can use
// the trailing 0 to indicate the end of file
//
// This is only needed if the file size is exactly a multiple of the page
// size. If it isn't, then the remains of the last page are 0 anyway.
string = mmap(NULL,
st.st_size + 1, // one extra byte
PROT_READ,
MAP_ANONYMOUS | MAP_PRIVATE,
-1, 0);
if(string == MAP_FAILED)
{
MSG("Couldn't mmap(anonymous) right before mmap(\"%s\")", filename);
goto done;
}
string = mmap(string, st.st_size,
PROT_READ,
MAP_FIXED | MAP_PRIVATE,
fd, 0);
if(string == MAP_FAILED)
{
MSG("Couldn't mmap(\"%s\")", filename);
goto done;
}
did_set_Nintrinsics_max = true;
result = read_cameramodel_from_string(model, Nintrinsics_max,
string,
// passing len==0 to use the \0 as the
// EOF marker. This is the more
// efficient path in
// read_cameramodel_from_string()
0);
done:
if(string != NULL && string != MAP_FAILED)
munmap(string, st.st_size+1);
if(fd >= 0)
close(fd);
if(Nintrinsics_max != NULL && !did_set_Nintrinsics_max)
*Nintrinsics_max = 0;
return result;
}
// 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
mrcal_cameramodel_VOID_t* mrcal_read_cameramodel_string(const char *string,
const int len)
{
mrcal_cameramodel_VOID_t* model = NULL;
bool result = read_cameramodel_from_string(&model, NULL,
string, len);
if(result) return model;
else return NULL;
}
mrcal_cameramodel_VOID_t* mrcal_read_cameramodel_file(const char* filename)
{
mrcal_cameramodel_VOID_t* model = NULL;
bool result = read_cameramodel_from_file(&model, NULL,
filename);
if(result) return model;
else return NULL;
}
void mrcal_free_cameramodel(mrcal_cameramodel_VOID_t** cameramodel)
{
free(*cameramodel);
*cameramodel = NULL;
}
bool mrcal_read_cameramodel_string_into(// out
mrcal_cameramodel_VOID_t* model,
// in,out
int* Nintrinsics_max,
// in
const char* string,
const int len)
{
return read_cameramodel_from_string(&model, Nintrinsics_max,
string, len);
}
bool mrcal_read_cameramodel_file_into (// out
mrcal_cameramodel_VOID_t* model,
// in,out
int* Nintrinsics_max,
// in
const char* filename)
{
return read_cameramodel_from_file(&model, Nintrinsics_max,
filename);
}
mrcal-2.5/choose_mrbuild.mk 0000664 0000000 0000000 00000001465 15123677724 0016031 0 ustar 00root root 0000000 0000000 # Use the local mrbuild or the system mrbuild or tell the user how to download
# it
ifneq (,$(wildcard mrbuild/))
MRBUILD_MK=mrbuild
MRBUILD_BIN=mrbuild/bin
else ifneq (,$(wildcard /usr/include/mrbuild/Makefile.common.header))
MRBUILD_MK=/usr/include/mrbuild
MRBUILD_BIN=/usr/bin
else
V := 1.14
SHA512 := 26a051c71544c97549e8bd9221fcce0b225948e55cea4c05ec447a31f67ec52a1648d61e978cc6a7b1317a46a13d50f159d98c35a46920dddcf0d4e89d63d16b
URL := https://github.com/dkogan/mrbuild/archive/refs/tags/v$V.tar.gz
TARGZ := mrbuild-$V.tar.gz
cmd := wget -O $(TARGZ) ${URL} && sha512sum --quiet --strict -c <(echo $(SHA512) $(TARGZ)) && tar xvfz $(TARGZ) && ln -fs mrbuild-$V mrbuild
$(error mrbuild not found. Either 'apt install mrbuild', or if not possible, get it locally like this: '${cmd}')
endif
mrcal-2.5/corresponding_icam_extrinsics.docstring 0000664 0000000 0000000 00000006224 15123677724 0022536 0 ustar 00root root 0000000 0000000 Return the icam_extrinsics corresponding to a given icam_intrinsics
SYNOPSIS
m = mrcal.cameramodel('xxx.cameramodel')
optimization_inputs = m.optimization_inputs()
icam_intrinsics = m.icam_intrinsics()
icam_extrinsics = \
mrcal.corresponding_icam_extrinsics(icam_intrinsics,
**optimization_inputs)
if icam_extrinsics >= 0:
rt_cam_ref_at_calibration_time = \
optimization_inputs['rt_cam_ref'][icam_extrinsics]
else:
rt_cam_ref_at_calibration_time = \
mrcal.identity_rt()
When calibrating cameras, each observation is associated with some camera
intrinsics (lens parameters) and some camera extrinsics (geometry). Those two
chunks of data live in different parts of the optimization vector, and are
indexed independently. If we have STATIONARY cameras, then each set of camera
intrinsics is associated with exactly one set of camera extrinsics, and we can
use THIS function to query this correspondence. If we have moving cameras, then
a single physical camera would have one set of intrinsics but many different
extrinsics, and this function will throw an exception.
Furthermore, it is possible that a camera's pose is used to define the reference
coordinate system of the optimization. In this case this camera has no explicit
extrinsics (they are an identity transfomration, by definition), and we return
-1, successfully.
In order to determine the camera mapping, we need quite a bit of context. If we
have the full set of inputs to the optimization function, we can pass in those
(as shown in the example above). Or we can pass the individual arguments that
are needed (see ARGUMENTS section for the full list). If the optimization inputs
and explicitly-given arguments conflict about the size of some array, the
explicit arguments take precedence. If any array size is not specified, it is
assumed to be 0. Thus most arguments are optional.
ARGUMENTS
- icam_intrinsics: an integer indicating which camera we're asking about
- **kwargs: if the optimization inputs are available, they can be passed-in as
kwargs. These inputs contain everything this function needs to operate. If we
don't have these, then the rest of the variables will need to be given
- Ncameras_intrinsics
Ncameras_extrinsics
- Nobservations_board
- Nobservations_point
optional integers; default to 0. These specify the sizes of various arrays in
the optimization. See the documentation for mrcal.optimize() for details
- indices_frame_camintrinsics_camextrinsics: array of dims (Nobservations_board,
3). For each observation these are an
(iframe,icam_intrinsics,icam_extrinsics) tuple. icam_extrinsics == -1
means this observation came from a camera in the reference coordinate system.
iframe indexes the "rt_ref_frame" array, icam_intrinsics indexes the
"intrinsics_data" array, icam_extrinsics indexes the "rt_cam_ref"
array
All of the indices are guaranteed to be monotonic. This array contains 32-bit
integers.
RETURNED VALUE
The integer reporting the index of the camera extrinsics in the optimization
vector. If this camera is at the reference of the coordinate system, return -1
mrcal-2.5/decode_observation_indices_points_triangulated.docstring 0000664 0000000 0000000 00000000071 15123677724 0026103 0 ustar 00root root 0000000 0000000 triangulated-points are experimental. Not yet documented
mrcal-2.5/doc/ 0000775 0000000 0000000 00000000000 15123677724 0013241 5 ustar 00root root 0000000 0000000 mrcal-2.5/doc/.dir-locals.el 0000664 0000000 0000000 00000002255 15123677724 0015676 0 ustar 00root root 0000000 0000000 ;;; Directory Local Variables
;; Useful to add links to a mrcal function or tool. These make the appropriate
;; text and an appropriate link
((org-mode . ((eval .
(progn
(defun insert-function (f)
(interactive (list (read-string "Function: ")))
(insert (format "[[file:mrcal-python-api-reference.html#-%1$s][=mrcal.%1$s()=]]"
f)))
(defun insert-tool (f)
(interactive (list (read-string "Tool: ")))
(insert (format "[[file:%1$s.html][=%1$s=]]"
f)))
(defun insert-file (f)
(interactive (list (read-string "File: ")))
(insert (format "[[https://www.github.com/dkogan/mrcal/blob/master/%1$s][=%1$s=]]"
f)))
(local-set-key (kbd "") 'insert-function)
(local-set-key (kbd "") 'insert-tool)
(local-set-key (kbd "") 'insert-file)
)))))
mrcal-2.5/doc/c-api.org 0000664 0000000 0000000 00000062740 15123677724 0014754 0 ustar 00root root 0000000 0000000 #+TITLE: mrcal C API
#+OPTIONS: toc:t
Internally, the [[file:python-api.org][Python functions]] use the mrcal C API. Only core functionality is
available in the C API (the Python API can do some stuff that the C API cannot),
but with time more and more stuff will be transitioned to a C-internal
representation. Today, end-to-end dense stereo processing in C is possible.
The C API consists of several headers:
- [[https://www.github.com/dkogan/mrcal/blob/master/basic-geometry.h][=basic-geometry.h=]]: /very/ simple geometry structures
- [[https://www.github.com/dkogan/mrcal/blob/master/poseutils.h][=poseutils.h=]]: pose and geometry functions
- [[https://www.github.com/dkogan/mrcal/blob/master/triangulation.h][=triangulation.h=]]: triangulation routines
- [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal.h=]]: lens models, projections, optimization
Most usages would simply =#include =, and this would include all the
headers. This is a C (not C++) library, so [[https://en.wikipedia.org/wiki/X_Macro][X macros]] are used in several places
for templating.
mrcal is a research project, so the capabilities and focus are still evolving.
Thus, the interfaces, /especially/ those in the C API are not yet stable. I do
try to maintain stability, but this is not fully possible, especially in the
higher-level APIs (=mrcal_optimize()= and =mrcal_optimizer_callback()=). For
now, assume that each major release breaks both the API and the ABI. The
migration notes for each release are described in the [[file:versions.org][relevant release notes]].
If you use the C APIs, shoot me an email to let me know, and I'll keep you in
mind when making API updates.
The best documentation for the C interfaces is the comments in the headers. I
don't want to write up anything complete and detailed until I'm sure the
interfaces are stable. The available functions are broken down into categories,
and described in a bit more detail here.
* Geometry structures
We have 3 structures in [[https://www.github.com/dkogan/mrcal/blob/master/basic-geometry.h][=basic-geometry.h=]]:
- =mrcal_point2_t=: a vector containing 2 double-precision floating-point
values. The elements can be accessed individually as =.x= and =.y= or as an
array =.xy[]=
- =mrcal_point3_t=: exactly like =mrcal_point2_t=, but 3-dimensional. A vector
containing 3 double-precision floating-point values. The elements can be
accessed individually as =.x= and =.y= and =.z= or as an array =.xyz[]=
- =mrcal_pose_t=: an unconstrained [[file:conventions.org::#pose-representation][6-DOF pose]]. Contains two sub-structures:
- =mrcal_point3_t r=: a [[https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector][Rodrigues rotation]]
- =mrcal_point3_t t=: a translation
Trivial mathematical operations are defined for these types:
- =double mrcal_point3_inner(const mrcal_point3_t a, const mrcal_point3_t b)=
- =double mrcal_point3_norm2(const mrcal_point3_t a)=
- =double mrcal_point3_mag (const mrcal_point3_t a)=
- =mrcal_point3_t mrcal_point3_add (const mrcal_point3_t a, const mrcal_point3_t b)=
- =mrcal_point3_t mrcal_point3_sub (const mrcal_point3_t a, const mrcal_point3_t b)=
- =mrcal_point3_t mrcal_point3_scale(const mrcal_point3_t a, const double s)=
- =mrcal_point3_t mrcal_point3_cross(const mrcal_point3_t a, const mrcal_point3_t b)=
And similar for =mrcal_point2_t=, except there's no =mrcal_point2_cross()=
And trivial printing operations are defined:
- =mrcal_point2_print(p)=
- =mrcal_point3_print(p)=
- =mrcal_Rt_print(Rt)=
- =mrcal_rt_print(rt)=
* Geometry functions
A number of utility functions are defined in [[https://www.github.com/dkogan/mrcal/blob/master/poseutils.h][=poseutils.h=]]. Each routine has two
forms:
- A =mrcal_..._full()= function that supports a non-contiguous memory layout for
each input and output
- A convenience =mrcal_...()= macro that wraps =mrcal_..._full()=, and expects
contiguous data. This has many fewer arguments, and is easier to call
Each data argument (input or output) has several items in the argument list:
- =double* xxx=: a pointer to the first element in the array
- =int xxx_stride0=, =int xxx_stride1=, ...: the strides, one per dimension
The strides are given in bytes, and work as expected. For a (for instance)
3-dimensional =xxx=, the element at =xxx[i,j,k]= would be accessible as
#+begin_src c
*(double*) &((char*)xxx)[ i*xxx_stride0 +
j*xxx_stride1 +
k*xxx_stride2 ]
#+end_src
These all have direct Python bindings. For instance the Python
[[file:mrcal-python-api-reference.html#-rt_from_Rt][=mrcal.rt_from_Rt()=]] function wraps =mrcal_rt_from_Rt()= C function.
There are also functions to solve the [[https://en.wikipedia.org/wiki/Orthogonal_Procrustes_problem][Orthogonal Procrustes Problem]]:
- =mrcal_align_procrustes_points_Rt01()=
- =mrcal_align_procrustes_vectors_R01()=
The [[https://www.github.com/dkogan/mrcal/blob/master/poseutils.h][=poseutils.h=]] header serves as the listing of available functions.
* Lens models
The lens model structures are defined here:
- =mrcal_lensmodel_type_t=: an enum decribing the lens model /type/. No
[[file:lensmodels.org::#representation][configuration]] is stored here.
- =mrcal_lensmodel_t=: a lens model type /and/ the [[file:lensmodels.org::#representation][configuration]] parameters. The
configuration lives in a =union= supporting all the known lens models
- =mrcal_lensmodel_metadata_t=: some [[file:lensmodels.org::#representation][metadata that decribes a model type]]. These
are inherent properties of a particular model type; answers questions like:
Can this model project behind the camera? Does it have an [[file:lensmodels.org::#core][intrinsics core]]?
Does it have gradients implemented?
The Python API describes a lens model with a [[file:lensmodels.org::#representation][string that contains the model type
and the configuration]], while the C API stores the same information in a
=mrcal_lensmodel_t=.
* Camera models
:PROPERTIES:
:CUSTOM_ID: cameramodel-in-c
:END:
We can also represent a full camera model in C. This is a lens model with a pose
and imager dimension: the full set of things in a [[file:cameramodels.org][=.cameramodel= file]]. The
definitions appear in [[https://www.github.com/dkogan/mrcal/blob/master/types.h][=mrcal/types.h=]]:
#+begin_src c
typedef struct
{
double rt_cam_ref[6];
unsigned int imagersize[2];
mrcal_lensmodel_t lensmodel;
double intrinsics[0];
} mrcal_cameramodel_VOID_t;
typedef union
{
double rt_cam_ref[6];
unsigned int imagersize[2];
mrcal_lensmodel_t lensmodel;
double intrinsics[4];
} mrcal_cameramodel_LENSMODEL_LATLON_t;
...
#+end_src
Note that =mrcal_cameramodel_VOID_t.intrinsics= has size 0 because the size of this
array depends on the specific lens model being used, and is unknown at compile
time.
So it is an error to define this on the stack. *Do not do this*:
#+begin_src c
void f(void)
{
mrcal_cameramodel_VOID_t model; // ERROR
}
#+end_src
If you need to define a known-at-compile-time model on the stack you can use the
[[https://github.com/dkogan/mrcal/blob/88e4c1df1c8cf535516719c5d4257ef49c9df1da/mrcal-types.h#L338][lensmodel-specific cameramodel types]]:
#+begin_src c
void f(void)
{
mrcal_cameramodel_LENSMODEL_OPENCV8_t model; // OK
}
#+end_src
This only exists for models that have a constant number of parameters; notably
there is no =mrcal_cameramodel_LENSMODEL_SPLINED_STEREOGRAPHIC_t=. When reading
a model from disk, mrcal dynamically allocates the right amount of memory, and
returns a =mrcal_cameramodel_VOID_t*=.
* Projections
The fundamental functions for projection and unprojection are defined here.
=mrcal_project()= is the main routine that implements the "forward" direction,
and is available for every camera model. This function can return gradients in
respect to the coordinates of the point being projected and/or in respect to the
intrinsics vector.
=mrcal_unproject()= is the reverse direction, and is implemented as a numerical
optimization to reverse the projection operation. Naturally, this is much slower
than =mrcal_project()=. Since =mrcal_unproject()= is implemented with a
nonlinear optimization, it has no gradient reporting. The Python
[[file:mrcal-python-api-reference.html#-unproject][=mrcal.unproject()=]] routine is higher-level, and it /does/ report gradients.
The gradients of the forward =mrcal_project()= operation are used in this
nonlinear optimization, so models that have no projection gradients defined do
not support =mrcal_unproject()=. The Python [[file:mrcal-python-api-reference.html#-unproject][=mrcal.unproject()=]] routine still
makes this work, using numerical differences for the projection gradients.
Simple, special-case lens models have their own projection and unprojection
functions defined:
#+begin_src c
void mrcal_project_pinhole(...);
void mrcal_unproject_pinhole(...);
void mrcal_project_stereographic(...);
void mrcal_unproject_stereographic(...);
void mrcal_project_lonlat(...);
void mrcal_unproject_lonlat(...);
void mrcal_project_latlon(...);
void mrcal_unproject_latlon(...);
#+end_src
These functions do the same thing as the general =mrcal_project()= and
=mrcal_unproject()= functions, but work much faster.
* Layout of the measurement and state vectors
The [[file:formulation.org][optimization routine]] tries to minimize the 2-norm of the measurement vector
$\vec x$ by moving around the state vector $\vec b$.
We select which parts of the optimization problem we're solving by setting bits
in the =mrcal_problem_selections_t= structure. This defines
- Which elements of the optimization vector are locked-down, and which are given
to the optimizer to adjust
- Whether we apply [[file:index.org::#Regularization][regularization]] to stabilize the solution
- Whether the chessboard should be assumed flat, or if we should optimize
[[file:formulation.org::#board-deformation][deformation]] factors
Thus 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 [[file:formulation.org::#board-deformation][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 [[file:formulation.org::#Regularization][regularization]] terms
Given =mrcal_problem_selections_t= and a vector $\vec b$ or $\vec x$, it is
useful to know where specific quantities lie inside those vectors. Here we have
4 sets of functions to answer such questions:
- =int mrcal_state_index_THING()=: Returns the index in the state vector $\vec
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 $\vec b$ that describe the given THING. THING is any
of
- intrinsics
- extrinsics
- frames
- points
- calobject_warp
- =int mrcal_measurement_index_THING()=: Returns the index in the measurement
vector $\vec 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 $\vec x$ that describe the given
THING. THING is any of
- boards
- points
- regularization
* State packing
The optimization routine works in the [[file:formulation.org::#state-packing][space of scaled parameters]], and several
functions are available to pack/unpack the state vector $\vec b$:
#+begin_src c
void mrcal_pack_solver_state_vector(...);
void mrcal_unpack_solver_state_vector(...);
#+end_src
* Optimization
The mrcal [[file:formulation.org][optimization routines]] are defined in [[https://www.github.com/dkogan/mrcal/blob/master/mrcal.h][=mrcal.h=]]. There are two primary
functions, each accessing a /lot/ of functionality, and taking /many/ arguments.
At this time, the prototypes will likely change in each release of mrcal, so try
not to rely on these being stable.
- =mrcal_optimize()= is the entry point to the optimization routine. This
function ingests the state, runs the optimization, and returns the optimal
state in the same variables. The optimization routine tries out different
values of the state vector by calling an optimization callback function to
evaluate each one.
- =mrcal_optimizer_callback()= provides access to the optimization callback
function standalone, /without/ being wrapped into the optimization loop
** Helper structures
This is correct as of mrcal 2.1. It may change in future releases.
We define some structures to organize the input to these functions. Each
observation has a =mrcal_camera_index_t= to identify the observing camera:
#+begin_src c
// 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
typedef struct
{
// indexes the intrinsics array
int intrinsics;
// indexes the extrinsics array. -1 means "at coordinate system reference"
int extrinsics;
} mrcal_camera_index_t;
#+end_src
When solving a vanilla calibration problem, we have a set of stationary cameras
observing a moving scene. By convention, in such a problem we set the reference
coordinate system to camera 0, so that camera has no extrinsics. So in a vanilla
calibration problem =mrcal_camera_index_t.intrinsics= will be in $[0,
N_\mathrm{cameras})$ and =mrcal_camera_index_t.extrinsics= will always be
=mrcal_camera_index_t.intrinsics - 1=.
When solving a vanilla structure-from-motion problem, we have a set of moving
cameras observing a stationary scene. Here =mrcal_camera_index_t.intrinsics=
would be in $[0, N_\mathrm{cameras})$ and =mrcal_camera_index_t.extrinsics=
would be specify the camera pose, unrelated to
=mrcal_camera_index_t.intrinsics=.
These are the limiting cases; anything in-between is allowed.
A board observation is defined by a =mrcal_observation_board_t=:
#+begin_src c
// 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;
#+end_src
And an observation of a discrete point is defined by a
=mrcal_observation_point_t=:
#+begin_src c
// 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;
// Observed pixel coordinates. This works just like elements of
// observations_board_pool:
//
// .x, .y are the pixel observations
// .z is the weight of the observation. Most of the weights are expected to
// be 1.0. Less precise observations have lower weights.
// .z<0 indicates that this is an outlier. This is respected on
// input
//
// Unlike observations_board_pool, outlier rejection is NOT YET IMPLEMENTED
// for points, so outlier points will NOT be found and reported on output in
// .z<0
mrcal_point3_t px;
} mrcal_observation_point_t;
#+end_src
Note that the details of the handling of discrete points may change in the
future.
We have =mrcal_problem_constants_t= to define some details of the optimization
problem. These are similar to =mrcal_problem_selections_t=, but consist of
numerical values, rather than just bits. Currently this structure contains valid
ranges for interpretation of discrete points. These may change in the future.
#+begin_src c
// 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 min,max distance of an observed discrete point from its observing
// camera. Any observation of a point outside this range will be penalized to
// encourage the optimizer to move the point into the acceptable range from the camera
double point_min_range, point_max_range;
} mrcal_problem_constants_t;
#+end_src
The optimization function returns most of its output in the same memory as its
input variables. A few metrics that don't belong there are returned in a
separate =mrcal_stats_t= structure:
#+begin_src c
// This structure is returned by the optimizer, and contains some statistics
// about the optimization
typedef struct
{
// generated by an X-macro
/* 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;
/* 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;
} mrcal_stats_t;
#+end_src
This contains some statistics describing the discovered optimal solution.
* Camera model reading/writing
:PROPERTIES:
:CUSTOM_ID: cameramodel-io-in-c
:END:
Simple interfaces for reading/writing [[file:cameramodels.org][=.cameramodel=]] data from C is available:
#+begin_src c
//// These allocate memory for the model; the caller MUST
//// mrcal_free_cameramodel(&model) when done. Return NULL on error
//
// 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
mrcal_cameramodel_VOID_t* mrcal_read_cameramodel_string(const char* string,
const int len);
mrcal_cameramodel_VOID_t* mrcal_read_cameramodel_file (const char* filename);
void mrcal_free_cameramodel(mrcal_cameramodel_VOID_t** cameramodel);
//// These read the model into a preallocated buffer *model. The given buffer is
//// big-enough for a model with *Nintrinsics_max intrinsics. Return true on
//// success. On failure, return false. If the error was a too-small
//// Nintrinsics_max, a big-enough value will be reported in *Nintrinsics_max.
//// Otherwise *Nintrinsics_max will be <= 0.
//
// 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
bool mrcal_read_cameramodel_string_into(// out
mrcal_cameramodel_VOID_t* model,
// in,out
int* Nintrinsics_max,
// in
const char* string,
const int len);
bool mrcal_read_cameramodel_file_into (// out
mrcal_cameramodel_VOID_t* model,
// in,out
int* Nintrinsics_max,
// in
const char* filename);
bool mrcal_write_cameramodel_file(const char* filename,
const mrcal_cameramodel_VOID_t* cameramodel);
#+end_src
This reads and write the [[#cameramodel-in-c][=mrcal_cameramodel_VOID_t= structures]]. Only the
=.cameramodel= file format is supported by these C functions. The Python API
supports more formats.
* Images
mrcal defines simple image types in [[https://www.github.com/dkogan/mrcal/blob/master/image.h][=mrcal/image.h=]]:
- =mrcal_image_int8_t=
- =mrcal_image_uint8_t=
- =mrcal_image_int16_t=
- =mrcal_image_uint16_t=
- =mrcal_image_int32_t=
- =mrcal_image_uint32_t=
- =mrcal_image_int64_t=
- =mrcal_image_uint64_t=
- =mrcal_image_float_t=
- =mrcal_image_double_t=
- =mrcal_image_bgr_t=
- =mrcal_image_void_t=
These are the basic not-necessarily-contiguous arrays. The =void= type is meant
to be used to accept arguments to functions that can operate on multiple types.
The =bgr= type is used for color images:
#+begin_src c
typedef struct { uint8_t bgr[3]; } mrcal_bgr_t;
#+end_src
Simple accessor and manipulation functions are available for each of these
types (except =void=; replacing each =T= below):
#+begin_src c
T* mrcal_image_T_at(mrcal_image_T_t* image, int x, int y);
const T* mrcal_image_T_at_const(const mrcal_image_T_t* image, int x, int y);
mrcal_image_T_t mrcal_image_T_crop(mrcal_image_T_t* image,
int x0, int y0,
int w, int h);
#+end_src
And for =uint8_t=, =uint16_t= and =mrcal_bgr_t= we can also read and write image
files:
#+begin_src c
bool mrcal_image_T_save (const char* filename, const mrcal_image_T_t* image);
bool mrcal_image_T_load( mrcal_image_T_t* image, const char* filename);
#+end_src
These use libraries for their core functionality, and are here for convenience.
These aren't interesting, or better than any other functions you may have
already; if you already have an image read/write implementation, use it. The
declarations are in [[https://www.github.com/dkogan/mrcal/blob/master/image.h][=mrcal/image.h=]], and the documentation lives there.
* Heat maps
mrcal can produce a colored visualization of any of the image types defined
above:
#+begin_src c
bool
mrcal_apply_color_map_T(
mrcal_image_bgr_t* out,
const mrcal_image_T_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);
#+end_src
* Dense stereo
:PROPERTIES:
:CUSTOM_ID: dense-stereo-in-c
:END:
A number of dense stereo routines are available. These make it possible to
implement a full mrcal dense stereo pipeline in C; an [[https://github.com/dkogan/mrcal/blob/master/doc/examples/dense-stereo-demo/dense-stereo-demo.cc][example is provided]]. The
available functions are declared in [[https://www.github.com/dkogan/mrcal/blob/master/stereo.h][=stereo.h=]]:
- =mrcal_rectified_resolution()= computes the resolution of the rectified system
from the resolution of the input. Usually =mrcal_rectified_system()= does this
internally, and there's no reason to call it directly. The Python wrapper is
[[file:mrcal-python-api-reference.html#-rectified_resolution][=mrcal.rectified_resolution()=]], and further documentation is in its docstring
- =mrcal_rectified_system2()= computes the geometry of the rectified system. The
Python wrapper is [[file:mrcal-python-api-reference.html#-rectified_system][=mrcal.rectified_system()=]], and further documentation is in
its docstring.
- =mrcal_rectification_maps()= computes the image transformation maps used to
compute the rectified images. To apply the maps, and actually remap the
images, [[https://docs.opencv.org/4.6.0/da/d54/group__imgproc__transform.html#gab75ef31ce5cdfb5c44b6da5f3b908ea4][the OpenCV =cv::remap()= function]] can be used. The Python wrapper is
[[file:mrcal-python-api-reference.html#-rectification_maps][=mrcal.rectification_maps()=]], and further documentation is in its docstring
- =mrcal_stereo_range_sparse()=, =mrcal_stereo_range_dense()= compute ranges
from disparities. The former function converts a set of discrete disparity
values, while the latter function processes a whole disparity image
* Triangulation
A number of triangulation routines are available in [[https://www.github.com/dkogan/mrcal/blob/master/triangulation.h][=triangulation.h=]]. These
estimate the position of the 3D point that produced a given pair of
observations.
* Sensor topology
=mrcal_traverse_sensor_links()= is available to compute the best path through a
graph of sensors, trying to maximize the common observations between each
successive pair of sensors in the path. This is used in the calibration seeding
routine to initialize a solve where every sensor's observations don't overlap
with every other sensor's observations.
* Heap routines
The [[https://www.github.com/dkogan/mrcal/blob/master/heap.h][=heap.h=]] header provides a simple min-priority-queue implementation. This
exposes the STL code in a C API. This is for the most part an internal
implementation detail of =mrcal_traverse_sensor_links()=, but could potentially
be useful somewhere on its own, so I'm exposing it here
* Python utilities
=mrcal_cameramodel_converter()= is a "converter" function that can be used with
=O&= conversions in [[https://docs.python.org/3/c-api/arg.html#c.PyArg_ParseTupleAndKeywords][=PyArg_ParseTupleAndKeywords()=]] calls. Can interpret either
path strings or [[file:mrcal-python-api-reference.html#cameramodel][=mrcal.cameramodel=]] objects as =mrcal_cameramodel_VOID_t= C
structures. Useful for writing Python extension modules for C code that uses
=mrcal_cameramodel_VOID_t=
mrcal-2.5/doc/cameramodels.org 0000664 0000000 0000000 00000016727 15123677724 0016423 0 ustar 00root root 0000000 0000000 #+TITLE: Camera model representation in memory and on disk
* Python interface
Interfacing with camera models is done in Python with the [[file:mrcal-python-api-reference.html#cameramodel][=mrcal.cameramodel=]]
class. This describes /one/ camera; a calibrated set of cameras is represented
by multiple objects. Each object always contains
- The =intrinsics=: lens parameters describing one of the [[file:lensmodels.org][lens models]]
- The =extrinsics=: a pose of the camera in space. This pose is represented as a
transformation between [[file:formulation.org::#world-geometry][some common /reference/ coordinate system]] and the
camera coordinate system. The specific meaning of the reference coordinate
system is arbitrary, but all the cameras in a calibrated set must be defined
in respect to the one common reference.
Each camera model object /may/ also contain:
- The =optimization_inputs=: all the data used to compute the model initially.
Used for the [[file:uncertainty.org][uncertainty computations]] and any after-the-fact analysis.
- The =valid_intrinsics_region=: a contour in the imager where the projection
behavior is "reliable". This is usually derived from the uncertainty plot, and
used as a shorthand. It isn't as informative as the uncertainty plot, but such
a valid-intrinsics contour is often convenient to have and to visualize.
* C interface
The [[file:c-api.org::#cameramodel-io-in-c][C API]] uses the [[https://github.com/dkogan/mrcal/blob/88e4c1df1c8cf535516719c5d4257ef49c9df1da/mrcal-types.h#L326][=mrcal_cameramodel_VOID_t= structure]] to represent a model. This
contains just the bare minimum:
- intrinsics (=mrcal_lensmodel_t lensmodel=, =double intrinsics[0]=)
- extrinsics (=double rt_cam_ref[6]=)
- imager size (=unsigned int imagersize[2]=)
Note that the intrinsics data has size 0 because the size of this array depends
on the specific lens model being used, and is unknown at compile time.
So it is an error to define this on the stack. *Do not do this*:
#+begin_src c
void f(void)
{
mrcal_cameramodel_VOID_t model; // ERROR
}
#+end_src
If you need to define a known-at-compile-time model on the stack you can use the
[[https://github.com/dkogan/mrcal/blob/88e4c1df1c8cf535516719c5d4257ef49c9df1da/mrcal-types.h#L338][lensmodel-specific cameramodel types]]:
#+begin_src c
void f(void)
{
mrcal_cameramodel_LENSMODEL_OPENCV8_t model; // OK
}
#+end_src
This only exists for models that have a constant number of parameters; notably
there is no =mrcal_cameramodel_LENSMODEL_SPLINED_STEREOGRAPHIC_t=. When reading
a model from disk, mrcal dynamically allocates the right amount of memory, and
returns a =mrcal_cameramodel_VOID_t*=.
The [[file:c-api.org::#cameramodel-io-in-c][C API]] has a simple interface for reading/writing =.cameramodel= data:
#+begin_src c
mrcal_cameramodel_VOID_t* mrcal_read_cameramodel_string(const char* string, int len);
mrcal_cameramodel_VOID_t* mrcal_read_cameramodel_file (const char* filename);
void mrcal_free_cameramodel(mrcal_cameramodel_VOID_t** cameramodel);
bool mrcal_write_cameramodel_file(const char* filename,
const mrcal_cameramodel_VOID_t* cameramodel);
#+end_src
* File formats
:PROPERTIES:
:CUSTOM_ID: cameramodel-file-formats
:END:
Several different file formats are supported:
- =.cameramodel=: the mrcal-native format, consisting of a plain text
representation of a Python =dict=. It supports /all/ the models, and is the
/only/ format supported by the [[file:c-api.org::#cameramodel-io-in-c][C API]], and is the only format that contains the
=optimization_inputs= and thus can be used for the [[file:uncertainty.org][uncertainty computations]].
- =.cahvor=: the legacy format available for compatibility with existing JPL
tools. If you don't need to interoperate with tools that require this format,
there's little reason to use it.
- [[https://github.com/ethz-asl/kalibr][kalibr]] =.yaml=: the [[https://github.com/ethz-asl/kalibr/wiki/Yaml-formats][format used by the kalibr toolkit]]. Unlike =.cameramodel=
files where one camera is described by one file, the =.yaml= files used by
kalibr are intended to describe multiple cameras. Thus only partial support is
available: we can convert to/from this format using the [[file:mrcal-to-kalibr.html][=mrcal-to-kalibr=]] and
[[file:mrcal-from-kalibr.html][=mrcal-from-kalibr=]] tools respectively. At this time the set of models
supported by both [[https://github.com/ethz-asl/kalibr][kalibr]] and mrcal contains [[file:lensmodels.org::#lensmodel-pinhole][=LENSMODEL_PINHOLE=]] and
[[file:lensmodels.org::#lensmodel-opencv][=LENSMODEL_OPENCV4=]] only.
- OpenCV/ROS =.yaml=: the [[https://wiki.ros.org/camera_calibration_parsers][format used by ROS and OpenCV]]. This supports
[[file:lensmodels.org::#lensmodel-opencv][=LENSMODEL_OPENCV5=]] and [[file:lensmodels.org::#lensmodel-opencv][=LENSMODEL_OPENCV8=]]. This format can describe a stereo
pair, but can /not/ describe an arbitrary set of N cameras. The reference
coordinate system is at the left-rectified camera.
The [[file:mrcal-python-api-reference.html#cameramodel][=mrcal.cameramodel=]] class will intelligently pick the correct file format
based on the data (if reading) and the filename (if writing). The
[[file:mrcal-to-cahvor.html][=mrcal-to-cahvor=]], [[file:mrcal-from-cahvor.html][=mrcal-from-cahvor=]], [[file:mrcal-to-kalibr.html][=mrcal-to-kalibr=]], [[file:mrcal-from-kalibr.html][=mrcal-from-kalibr=]]
and [[file:mrcal-from-ros.html][=mrcal-from-ros=]] can convert between the different file formats. There's no
=mrcal-to-ros= at this time because the behavior of such a tool isn't
well-defined. Talk to me if this would be useful to you, to clarify what it
should do, exactly.
* Sample usages
See the [[file:mrcal-python-api-reference.html#cameramodel][API documentation]] for usage details.
** Grafting two models
A trivial example to
- read two models from disk
- recombine into a joint model that uses the lens parameters from one model with
geometry from the other
- write to disk
#+begin_src python
import mrcal
model_for_intrinsics = mrcal.cameramodel('model0.cameramodel')
model_for_extrinsics = mrcal.cameramodel('model1.cameramodel')
model_joint = mrcal.cameramodel( model_for_intrinsics )
extrinsics = model_for_extrinsics.rt_cam_ref()
model_joint.rt_cam_ref(extrinsics)
model_joint.write('model-joint.cameramodel')
#+end_src
This is the basic operation of the [[file:mrcal-graft-models.html][=mrcal-graft-models= tool]].
** Re-optimizing a model
To re-optimize a model from its =optimization_inputs=:
#+begin_src python
import mrcal
m = mrcal.cameramodel('camera.cameramodel')
optimization_inputs = m.optimization_inputs()
mrcal.optimize(**optimization_inputs)
model_reoptimized = \
mrcal.cameramodel( optimization_inputs = m.optimization_inputs(),
icam_intrinsics = m.icam_intrinsics() )
#+end_src
This is the basic operation of the [[https://github.com/dkogan/mrcal/blob/master/analyses/mrcal-reoptimize][=analyses/mrcal-reoptimize= tool]].
Here we asked mrcal to re-optimize the data used to compute the given model
originally. We didn't make any changes to the inputs, and we should already have
an optimal solution, so this re-optimized model would be the same as the initial
one. But we could tweak optimization problem before reoptimizing, and this would
give us an nice way to observe the effects of those changes. We can add input
noise or change the lens model or [[file:formulation.org::#Regularization][regularization terms]] or anything else.
mrcal-2.5/doc/commandline-tools.org 0000664 0000000 0000000 00000011113 15123677724 0017373 0 ustar 00root root 0000000 0000000 #+TITLE: mrcal commandline tools
A number of commandline tools are available for common tasks, obviating the need
to write any code. The available tools, with links to their manpages:
* Calibration
- [[file:mrcal-calibrate-cameras.html][=mrcal-calibrate-cameras=]]: Calibrate N cameras. This is the main tool to solve
"calibration" problems, and a [[file:how-to-calibrate.org][how-to page]] describes this specific use case.
- [[file:mrcal-cull-corners.html][=mrcal-cull-corners=]]: Filters a =corners.vnl= on stdin to cut out some points.
Used primarily for testing
* Visualization
:PROPERTIES:
:CUSTOM_ID: commandline-tools-visualization
:END:
- [[file:mrcal-show-projection-diff.html][=mrcal-show-projection-diff=]]: visualize the difference in projection between N
models
- [[file:mrcal-show-projection-uncertainty.html][=mrcal-show-projection-uncertainty=]]: visualize the expected projection error
due to uncertainty in the calibration-time input
- [[file:mrcal-show-valid-intrinsics-region.html][=mrcal-show-valid-intrinsics-region=]]: visualize the region where a model's
intrinsics are valid
- [[file:mrcal-show-geometry.html][=mrcal-show-geometry=]]: show a visual representation of the geometry
represented by some camera models on disk, and optionally, the chessboard
observations used to compute that geometry
- [[file:mrcal-show-distortion-off-pinhole.html][=mrcal-show-distortion-off-pinhole=]]: visualize the deviation of a specific
lens model from a pinhole model
- [[file:mrcal-show-splined-model-correction.html][=mrcal-show-splined-model-correction=]]: visualize the projection corrections
defined by a splined model
- [[file:mrcal-show-residuals-board-observation.html][=mrcal-show-residuals-board-observation=]]: visualize calibration residuals for
one or more observations of a board
- [[file:mrcal-show-residuals.html][=mrcal-show-residuals=]]: visualize calibration residuals in an imager
- [[file:mrcal-show-model-resolution.html][=mrcal-show-model-resolution=]]: visualize the angular resolution across the
imager
* Camera model manipulation
- [[file:mrcal-convert-lensmodel.html][=mrcal-convert-lensmodel=]]: Fits the behavior of one lens model to another
- [[file:mrcal-graft-models.html][=mrcal-graft-models=]]: Combines the intrinsics of one cameramodel with the
extrinsics of another
- [[file:mrcal-to-cahvor.html][=mrcal-to-cahvor=]]: Converts a model stored in the native =.cameramodel= file
format to the =.cahvor= format. This exists for compatibility only, and does
not touch the data: any lens model may be used
- [[file:mrcal-from-cahvor.html][=mrcal-from-cahvor=]]: Converts a model stored in the =.cahvor= file format to
the =.cameramodel= format. This exists for compatibility only, and does not
touch the data: any lens model may be used
- [[file:mrcal-to-kalibr.html][=mrcal-to-kalibr=]]: Converts a model stored in the native =.cameramodel= file
format to the =.yaml= format used by [[https://github.com/ethz-asl/kalibr][kalibr]].
- [[file:mrcal-from-kalibr.html][=mrcal-from-kalibr=]]: Converts a model stored in =.yaml= files used by [[https://github.com/ethz-asl/kalibr][kalibr]]
to the =.cameramodel= format.
- [[file:mrcal-from-ros.html][=mrcal-from-ros=]]: Converts a model stored in =.yaml= files used by [[https://www.ros.org/][ROS]] and
[[https://opencv.org/][OpenCV]] to the =.cameramodel= format.
* Reprojection
- [[file:mrcal-reproject-image.html][=mrcal-reproject-image=]]: Given image(s) and lens model(s), produces a new set
of images that observe the same scene with a different model. Several flavors
of functionality are included here, such as undistortion-to-pinhole,
re-rotation, and remapping to infinity.
- [[file:mrcal-reproject-points.html][=mrcal-reproject-points=]]: Given two lens models and a set of pixel coodinates,
maps them from one lens model to the other
* Miscellaneous utilities
- [[file:mrcal-is-within-valid-intrinsics-region.html][=mrcal-is-within-valid-intrinsics-region=]]: Augments a vnlog of pixel
coordinates with a column indicating whether or not each point lies within
the valid-intrinsics region
* Stereo and triangulation
- [[file:mrcal-stereo.html][=mrcal-stereo=]]: Dense stereo processing. Given pairs of images captured by a
given pair of camera models, runs stereo matching to produce disparity and
range images.
- [[file:mrcal-triangulate.html][=mrcal-triangulate=]]: Sparse stereo processing. Given a pair of images captured
by a given pair of camera models, reports the range to a queried feature and
its sensitivities to all the inputs. Very useful in diagnosing accuracy issues
in the intrinsics and extrinsics.
mrcal-2.5/doc/conventions.org 0000664 0000000 0000000 00000025461 15123677724 0016327 0 ustar 00root root 0000000 0000000 #+TITLE: mrcal conventions
#+OPTIONS: toc:t
* Terminology
:PROPERTIES:
:CUSTOM_ID: terminology
:END:
Some terms in the documentation and sources can have ambiguous meanings, so I
explicitly define them here
- *calibration*: the procedure used to compute the lens parameters and geometry
of a set of cameras; or the result of this procedure. Usually this involves a
stationary set of cameras observing a moving object.
- [[file:formulation.org::#calibration-object][*calibration object*]] or *chessboard* or *board*: these are used more or less
interchangeably. They refer to the known-geometry object observed by the
cameras, with those observations used as input during calibration
- *camera model*: this is used to refer to the intrinsics (lens parameters) and
extrinsics (geometry) together
- *confidence*: a measure of dispersion of some estimator. Higher confidence
implies less dispersion. Used to describe the [[file:uncertainty.org][calibration quality]]. Inverse of
*uncertainty*
- *extrinsics*: the pose of a camera in respect to some fixed coordinate system
- *frames*: in the context of [[file:formulation.org][mrcal's optimization]] these refer to an array of
poses of the observed chessboards
- *intrinsics*: the parameters describing the behavior of a lens. The pose of
the lens is independent of the intrinsics
- *measurements* or *residuals*: used more or less interchangeably. This is the
vector whose norm the [[file:formulation.org][optimization algorithm]] is trying to minimize. mrcal
refers to this as $\vec x$, and it primarily contains differences between
observed and predicted pixel observations
- *projection*: a mapping of a point in space in the camera coordinate system
to a pixel coordinate where that point would be observed by a given camera
- *pose*: a 6 degree-of-freedom vector describing a position and an orientation
- *SFM*: structure from motion. This is the converse of "calibration": we
observe a stationary scene from a moving camera to compute the geometry of the
scene
- *state*: the vector of parameters that the [[file:formulation.org][mrcal optimization algorithm]] moves
around as it searches for the optimum. mrcal refers to this as $\vec b$
- *uncertainty*: a measure of dispersion of some estimator. Higher uncertainty
implies more dispersion. Used to describe the [[file:uncertainty.org][calibration quality]]. Inverse of
*confidence*
- *unprojection*: a mapping of a pixel coordinate back to a point in space in
the camera coordinate system that would produce an observation at that pixel.
Unprojection is only unique up-to scale
* Symbols
** Geometry
- $\vec q$ is a 2-dimensional vector representing a pixel coordinate: $\left( x,y \right)$
- $\vec v$ is a 3-dimensional vector representing a /direction/ $\left( x,y,z
\right)$ in space. $\vec v$ is unique only up-to-length. In a camera's
coordinate system we have $\vec q = \mathrm{project}\left(\vec v \right)$
- $\vec p$ is a 3-dimensional vector representing a /point/ $\left( x,y,z
\right)$ in space. Unlike $\vec v$, $\vec p$ has a defined range. Like $\vec
v$ we have $\vec q = \mathrm{project}\left(\vec p \right)$
- $\vec u$ is a 2-dimensional vector representing a [[file:lensmodels.org::#lensmodel-stereographic][normalized stereographic projection]]
** Optimization
:PROPERTIES:
:CUSTOM_ID: symbols-optimization
:END:
The core of the [[file:formulation.org][mrcal calibration routine]] is a nonlinear least-squares
optimization
\[
\min_{\vec b} E = \min_{\vec b} \left \Vert \vec x \left( \vec b \right) \right \Vert ^2
\]
Here we have
- $\vec b$ is the vector of parameters being optimized. Earlier versions of
mrcal used $\vec p$ for this, but it clashed with $\vec p$ referring to points
in space, which wasn't always clear from context. Some older code or
documentation may still use $\vec p$ to refer to optimization state
- $\vec x$ is the vector of /measurements/ describing the error of the solution
at some hypothesis $\vec b$
- $E$ is the cost function being optimized. $E \equiv \left \Vert \vec x \right \Vert ^2$
- $\vec J$ is the /jacobian/ matrix. This is the matrix $\frac{ \partial \vec x
}{ \partial \vec b }$. Usually this is large and sparse.
* Image coordinate system
:PROPERTIES:
:CUSTOM_ID: image-coordinate-system
:END:
As with most image-oriented tools, mrcal places the image (0,0) at the *center
of the top-left pixel*. Thus the top-left corner of the image is at (-0.5,-0.5).
Other possible choices /not/ used by mrcal are
- what e.g. [[https://boofcv.org][BoofCV]] does: (0,0) is at the top-left corner of the top-left pixel
- what e.g. [[https://en.wikipedia.org/wiki/OpenGL][OpenGL]] does: (0,0) is at the bottom-left corner of the bottom-left pixel
* Camera coordinate system
:PROPERTIES:
:CUSTOM_ID: camera-coordinate-system
:END:
mrcal uses right-handed coordinate systems. No convention is assumed for the
world coordinate system. The canonical /camera/ coordinate system has $x$ and
$y$ as with pixel coordinates in an image: $x$ is to the right and $y$ is down.
$z$ is then forward to complete the right-handed system of coordinates.
* Transformation naming
:PROPERTIES:
:CUSTOM_ID: transformation-naming
:END:
We describe transformations as mappings between a representation of a point in
one coordinate system to a representation of the /same/ point in another
coordinate system. =T_AB= is a transformation from coordinate system =B= to
coordinate system =A=. These chain together nicely, so if we know the
transformation between =A= and =B= and between =B= and =C=, we can transform a
point represented in =C= to =A=: =x_A = T_AB T_BC x_C = T_AC x_C=. And =T_AC =
T_AB T_BC=.
* Pose representation
:PROPERTIES:
:CUSTOM_ID: pose-representation
:END:
Various parts of the toolkit have preferred representations of pose, and mrcal
has functions to convert between them. Available representations are:
- =Rt=: a (4,3) numpy array with a (3,3) rotation matrix concatenated with a
(1,3) translation vector:
\[ \begin{bmatrix} R \\ \vec t^T \end{bmatrix} \]
This form is easy to work with, but there are implied constraints: most (4,3)
numpy arrays are /not/ valid =Rt= transformations.
- =rt=: a (6,) numpy array with a (3,) vector representing a [[https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation#Rotation_vector][Rodrigues rotation]]
concatenated with another (3,) vector, representing a translation:
\[ \left[ \vec r^T \quad \vec t^T \right] \]
This form requires more computations to deal with, but has no implied
constraints: /any/ (6,) numpy array is a valid =rt= transformation. Thus this
is the form used inside the [[file:formulation.org][mrcal optimization routine]].
- =qt=: a (7,) numpy array with a (4,) vector representing a unit quaternion
$\left(w,x,y,z\right)$ concatenated with another (3,) vector, representing a
translation:
\[ \left[ \vec q^T \quad \vec t^T \right] \]
mrcal doesn't use quaternions anywhere, and this exists only for
interoperability with other tools.
Each of these represents a transformation =rotate(x) + t=.
Since a pose represents a transformation between two coordinate systems, the
toolkit generally refers to a pose as something like =Rt_AB=, which is an
=Rt=-represented transformation to convert a point to a representation in the
coordinate system =A= from a representation in coordinate system =B=.
A Rodrigues rotation vector =r= represents a rotation of =length(r)= radians
around an axis in the direction =r=. Converting between =R= and =r= is done via
the [[https://en.wikipedia.org/wiki/Rodrigues%27_rotation_formula][Rodrigues rotation formula]]: using the [[file:mrcal-python-api-reference.html#-r_from_R][=mrcal.r_from_R()=]] and
[[file:mrcal-python-api-reference.html#-R_from_r][=mrcal.R_from_r()=]] functions. For translating /poses/, not just rotations, use
[[file:mrcal-python-api-reference.html#-rt_from_Rt][=mrcal.rt_from_Rt()=]] and [[file:mrcal-python-api-reference.html#-Rt_from_rt][=mrcal.Rt_from_rt()=]].
* Linear algebra
:PROPERTIES:
:CUSTOM_ID: linear-algebra
:END:
mrcal follows the usual linear algebra convention of /column/ vectors. So
rotating a vector using a rotation matrix is a matrix-vector multiplication
operation: $\vec b = R \vec a$ where both $\vec a$ and $\vec b$ are column
vectors.
However, numpy prints vectors (1-dimensional objects), as /row/ vectors, so the
code treats 1-dimensional objects as transposed vectors. In the code, the above
rotation would be implemented equivalently: $\vec b^T = \vec a^T R^T$. The
[[file:mrcal-python-api-reference.html#-rotate_point_R][=mrcal.rotate_point_R()=]] and [[file:mrcal-python-api-reference.html#-transform_point_Rt][=mrcal.transform_point_Rt()=]] functions handle this
transparently.
A similar issue is that numpy follows the linear algebra convention of indexing
arrays with =(index_row, index_column)= and representing array sizes with
=(height, width)=. This runs against the /other/ convention of referring to
pixel coordinates as =(x,y)= and image dimensions as =(width, height)=. Whenever
possible, mrcal places the =x= coordinate first (as in the latter), but when
interacting directly with numpy, it must place the =y= coordinate first.
/Usually/ =x= goes first. In any case, the choice being made is very clearly
documented, so when in doubt, pay attention to the docs.
When computing gradients mrcal places the dependent variables in the leading
dimensions, and the independent variables in the trailing dimensions. So if we
have $\vec b = R \vec a$, then
\[ R = \frac{ \partial \vec b }{ \partial \vec a } =
\left[ \begin{aligned} \frac{ \partial b_0 }{ \partial \vec a } \\
\frac{ \partial b_1 }{ \partial \vec a } \\
\frac{ \partial b_2 }{ \partial \vec a } \end{aligned} \right] =
\left[ \frac{ \partial \vec b }{ \partial a_0 } \quad
\frac{ \partial \vec b }{ \partial a_1 } \quad
\frac{ \partial \vec b }{ \partial a_2 } \right]
\]
$\frac{ \partial b_i }{ \partial \vec a }$ is a (1,3) row vector and $\frac{
\partial \vec b }{ \partial a_i }$ is a (3,1) column vector.
* Implementation
:PROPERTIES:
:CUSTOM_ID: implementation
:END:
The core of mrcal is written in C, but most of the API is currently available in
Python only. The Python wrapping is done via the [[https://github.com/dkogan/numpysane/blob/master/README-pywrap.org][=numpysane_pywrap=]] library,
which makes it simple to make consistent Python interfaces /and/ provides
[[https://numpy.org/doc/stable/user/basics.broadcasting.html][broadcasting]] support.
The Python layer uses [[https://numpy.org/][=numpy=]] and [[https://github.com/dkogan/numpysane/][=numpysane=]] heavily. All the plotting is done
with [[https://github.com/dkogan/gnuplotlib][=gnuplotlib=]]. [[https://opencv.org/][OpenCV]] is used a bit, but /only/ in the Python layer, less
and less over time (their C APIs are gone, and the C++ APIs are unstable).
mrcal-2.5/doc/copyrights.org 0000664 0000000 0000000 00000004407 15123677724 0016152 0 ustar 00root root 0000000 0000000 #+TITLE: Copyrights and licenses
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
A number of free-software projects are included as source in the toolkit. These
are:
* [[https://github.com/dkogan/libminimath][libminimath]]
mrcal uses some tiny-matrix linear algebra routines implemented by this library.
This is a header-only library, so we're not "linking" to it. I'm not modifying
the sources, so using the LGPL here is fine, and doesn't trigger the copyleft
provisions. Copyright 2011 Oblong Industries. This library is distributed under
the terms of the GNU LGPL
* opencv projection
The [[https://www.github.com/dkogan/mrcal/blob/master/mrcal-opencv.c][opencv projection function]] is a cut-down implementation from the OpenCV
project. Distributed under an BSD-style license. Copyright (C) 2000-2008, Intel
Corporation, all rights reserved. Copyright (C) 2009, Willow Garage Inc., all
rights reserved.
* =mrcal_r_from_R()=
The [[https://www.github.com/dkogan/mrcal/blob/master/poseutils-opencv.c][=mrcal_r_from_R()= sources]] originated in the OpenCV project. Distributed
under an BSD-style license. Copyright (C) 2000-2008, Intel Corporation, all
rights reserved. Copyright (C) 2009, Willow Garage Inc., all rights reserved.
* [[file:mrcal-python-api-reference.html#-quat_from_R][=mrcal.quat_from_R()=]]
The [[https://www.github.com/dkogan/mrcal/blob/master/mrcal/_poseutils_scipy.py][=mrcal.quat_from_R()= sources]] came from scipy. Distributed under a
BSD-style license. Copyright (c) 2001-2002 Enthought, Inc. 2003-2019, SciPy
Developers.
* pydoc
[[https://www.github.com/dkogan/mrcal/blob/master/doc/pydoc.py][=doc/pydoc.py=]] is a copy of the sources in the Python project. This is used to
extract the docstrings into the .html reference documentation. Minor
modifications were made to fit with mrcal's code organization and html styling.
Distributed under the PYTHON SOFTWARE FOUNDATION LICENSE VERSION 2. Copyright
(c) 2001, 2002, 2003, 2004, 2005, 2006, 2007, 2008, 2009, 2010, 2011, 2012,
2013, 2014 Python Software Foundation; All Rights Reserved.
mrcal-2.5/doc/data/ 0000775 0000000 0000000 00000000000 15123677724 0014152 5 ustar 00root root 0000000 0000000 mrcal-2.5/doc/data/README 0000664 0000000 0000000 00000000501 15123677724 0015026 0 ustar 00root root 0000000 0000000 All images (board and views) are taken with
- F22
- ISO1600
This allows fast shutter speeds to minimize motion blur and a big depth of field
to minimize focus blur.
The view of downtown is from the catwalk over Figueroa St. between 2nd and 3rd.,
looking S. At roughly 34.0558, -118.2539. Spacing between cameras: 7ft
mrcal-2.5/doc/data/figueroa-overpass-looking-S/ 0000775 0000000 0000000 00000000000 15123677724 0021453 5 ustar 00root root 0000000 0000000 mrcal-2.5/doc/data/figueroa-overpass-looking-S/features.imgany.inputimage.vnl 0000664 0000000 0000000 00000020715 15123677724 0027443 0 ustar 00root root 0000000 0000000 # x1 y1 x2 y2
2797.320834 2817.122168 2516.397326 2809.480939
2745.412449 2817.276777 2466.424937 2807.753364
3498.985683 3048.130530 3145.013906 3069.718464
3592.350428 3199.133514 3198.330034 3227.890159
4905.693042 2701.936035 4681.926645 2767.586994
2434.247258 2811.970419 2170.132988 2791.710076
3730.066112 3207.530405 3336.811932 3245.156052
2452.640555 2771.603588 2197.820494 2753.630116
4328.868094 2650.979423 4096.657287 2696.618361
3767.318480 3134.347557 3395.789230 3172.220122
3483.817362 3094.172913 3117.605605 3115.684005
3656.422647 3063.942469 3300.637299 3094.070411
3691.802597 3031.321826 3346.234850 3062.452762
1817.818460 2560.366416 1638.975124 2533.481899
2613.595171 3470.351539 2173.198731 3426.746511
2540.022187 2763.121854 2283.297707 2748.025846
2527.254718 3462.609402 2095.163809 3413.788442
3892.180131 3040.984626 3551.024146 3082.676696
2694.465421 3709.950822 2188.495744 3658.863688
3716.378392 3144.804033 3339.806903 3179.890912
3741.095485 3655.022688 3225.239518 3705.325082
4874.231404 2795.807919 4648.597533 2863.227763
5049.287623 2880.232487 4820.376166 2959.761653
3782.276973 3706.934586 3253.441635 3762.806798
4384.005176 2652.736098 4144.937807 2698.186773
3474.235763 2833.309137 3180.212854 2851.381747
3560.293635 2668.947875 3313.530662 2688.650739
4078.506177 2983.336012 3762.287854 3031.547994
2536.030989 3655.243306 2056.228421 3594.459583
3981.586581 3143.885375 3616.590556 3194.510187
2509.842608 3738.673199 2012.272528 3669.792424
3588.326048 3067.263473 3230.317974 3094.035288
2706.165027 3471.897956 2258.893409 3434.838746
1701.951685 2648.483490 1513.021920 2614.966244
2568.098052 3516.756483 2119.776396 3467.182543
3375.055448 2566.920512 3155.574880 2580.227377
4085.218633 3155.783624 3722.814205 3213.033498
3644.948617 3670.842182 3122.186061 3711.646810
1604.206593 2499.253672 1449.791963 2469.883446
2083.205537 3412.655339 1701.809198 3340.238383
2162.337989 3422.311853 1771.122114 3353.375947
2532.608994 3597.978315 2067.407520 3540.710163
2203.063677 2844.330228 1943.754995 2815.280661
2431.548721 3794.412642 1928.068455 3714.646541
2143.415522 3491.936763 1738.402497 3416.224594
2465.462981 3501.579210 2029.247411 3445.844205
2449.084604 3553.762878 2001.368174 3493.363826
2675.309121 3519.265128 2218.361721 3477.185357
3093.042574 2588.934750 2868.770603 2594.313442
1430.470255 2411.751408 1285.891960 2380.853939
2175.491770 2843.095393 1918.211278 2813.277839
2382.917631 3451.454903 1965.500967 3393.753503
2026.627328 3702.842321 1587.374878 3600.376466
3851.901166 3060.808614 3503.461195 3101.499261
2369.611260 3302.186843 1989.098186 3253.580457
2608.546981 3807.433070 2086.166772 3742.291986
1595.689720 3564.961417 1235.609698 3452.704594
1930.538636 3385.854740 1569.725018 3307.849541
2607.355859 2856.653652 2323.587176 2841.002189
1522.023024 3615.963175 1160.809399 3494.565521
2359.351383 3556.920926 1918.985578 3490.044475
4164.493970 3039.545314 3838.273781 3094.844984
1696.831326 3339.406693 1370.571436 3255.277960
2740.496068 3486.068150 2287.992991 3450.371126
4232.618904 3128.717810 3887.143102 3192.743818
4157.525519 3113.656565 3810.896293 3172.611992
4356.617989 3530.428084 3913.620250 3630.512786
2420.656668 3212.582756 2057.864595 3172.352534
1960.705983 3710.678106 1527.112568 3603.374397
2294.404108 3410.026788 1894.188419 3350.141014
2028.608665 3449.159707 1644.050918 3370.476177
2350.438952 3607.778386 1898.821744 3536.221616
2725.919046 3555.918054 2256.702400 3515.892476
2739.690971 2595.770667 2520.362443 2591.059743
2304.808671 3711.279550 1833.376506 3628.295964
2264.569646 3734.886621 1792.069300 3646.660428
4535.526160 3683.013752 4070.220566 3811.880529
1717.860594 3649.695132 1325.996209 3534.251227
3997.668493 3019.604957 3667.556010 3065.859565
2071.323928 3632.309230 1642.191190 3539.813389
2513.407369 3802.671186 2000.562338 3729.536767
1821.397937 3678.523314 1410.841144 3565.956635
3714.479894 2846.795550 3421.438802 2874.878572
2353.646682 3780.536161 1861.550163 3694.982873
2201.517636 2912.926817 1926.051453 2880.134552
5451.089926 2777.712350 5256.210561 2864.328293
1970.901162 3172.724004 1653.851849 3114.416511
2358.454094 2737.028150 2116.909462 2717.435187
2883.011728 3644.985868 2381.603875 3613.739207
5412.007185 2857.524383 5211.492868 2947.298402
1812.110604 3512.726140 1437.229853 3416.661063
4271.372218 3555.024271 3814.075090 3649.990985
2052.863460 3568.196106 1640.220791 3480.262080
1462.554682 3443.826744 1141.890729 3339.424269
4435.665692 3508.500848 4005.674339 3613.256216
2273.281371 2409.579063 2115.129726 2397.818998
1832.970339 3470.644151 1465.004416 3379.781044
4486.874030 3548.471851 4051.712136 3660.830669
1703.182027 3384.104704 1367.216303 3295.844045
1559.091597 3345.474140 1246.249224 3255.203602
2168.824393 2949.042035 1887.035058 2913.405398
1634.932780 3325.898410 1317.617331 3240.644387
4646.642966 3613.621995 4211.718512 3745.456373
1940.011704 3475.453976 1559.197668 3389.630690
1741.270930 3598.793463 1356.610471 3490.153683
2706.287077 3754.441330 2188.571562 3701.626312
3521.228559 3520.632067 3037.923555 3549.064254
1760.701881 3662.048442 1361.193831 3547.762316
4107.290054 2756.529126 3848.779387 2796.497473
5432.132065 2766.889806 5238.312330 2851.826100
1619.354668 3361.760990 1297.196623 3272.133740
2294.427385 2756.999741 2051.109869 2734.600441
2216.224007 3430.260441 1817.891903 3363.949318
4349.856021 2431.697738 4156.443447 2470.460195
2686.832665 3815.609171 2155.697898 3757.154626
1541.525618 3304.904567 1238.148251 3218.011342
4273.010062 2945.795528 3978.123775 3000.725690
3774.740745 2481.519871 3576.594456 2504.821160
1349.411935 2322.999889 1218.244821 2292.476433
4407.337928 2453.905590 4209.900497 2495.443294
2450.241632 3734.834665 1958.963424 3661.259357
4007.872808 3275.695145 3607.889513 3334.006990
4590.331868 3666.143935 4135.339660 3798.501343
1905.551257 3638.353893 1493.756552 3534.859315
2120.154812 3547.870233 1705.405149 3465.870685
1448.813526 3569.919386 1105.545293 3450.696712
2467.250163 2615.890454 2251.836526 2603.052272
2869.192382 3600.317757 2379.700249 3569.921282
2934.465258 2917.582982 2621.876869 2913.024776
4594.266788 3860.751438 4087.970058 4011.811700
1513.761981 3336.322209 1207.354205 3245.630051
2090.548901 2537.277324 1909.714690 2517.429421
1855.990583 3629.215213 1451.879528 3523.397671
2803.746154 2465.182085 2618.586361 2463.932697
3495.319011 3318.976311 3066.885098 3342.735788
1235.192994 2445.582679 1102.065916 2410.575734
3820.368322 3698.246868 3308.250732 3757.286013
2650.773337 3675.256811 2156.774044 3623.179768
1625.641071 2497.496251 1471.326372 2468.192675
3353.079663 2872.845268 3047.385880 2885.893018
4162.887776 2854.132549 3885.552464 2900.009772
2589.837931 2470.537916 2409.281928 2464.251451
3564.084749 2320.771490 3407.928089 2337.524983
4753.816893 2544.463455 4550.450915 2596.472129
1227.263850 2380.909443 1096.052485 2347.829836
5175.684647 2896.286071 4948.162643 2983.183791
4619.735150 3194.287540 4287.887130 3283.375283
2837.396490 3707.051855 2322.247823 3668.713450
1504.490469 3410.335386 1185.066886 3310.980764
1938.440894 2470.430652 1773.744296 2449.200131
3544.049578 3619.219293 3033.958423 3649.325864
1544.881775 3543.348119 1195.256034 3431.163897
4673.646775 3631.730868 4237.781317 3767.218060
2853.480575 2364.518992 2695.425164 2365.301328
4404.208774 3715.100077 3915.819285 3834.206134
2969.673316 2681.273959 2721.038995 2682.241886
4835.371422 2534.273503 4641.770169 2587.430493
2085.852605 3226.789004 1746.611074 3169.432938
4565.409798 3525.173531 4143.905262 3641.433450
2821.963962 3785.140750 2288.065779 3741.155271
2975.949697 2610.889985 2746.918065 2612.582795
2927.188621 2971.567609 2600.791114 2965.528398
5089.771204 2698.410882 4896.791842 2764.994721
3842.668489 3858.029023 3274.118317 3925.424886
2793.124072 3967.522737 2215.768119 3911.146696
2306.728089 3259.152837 1941.379817 3210.122382
3543.786980 3594.268116 3040.610712 3623.624493
4532.520949 3483.456836 4119.120988 3593.734187
3988.699840 2699.601328 3741.101120 2733.627107
1224.302115 2855.135663 1027.812510 2797.805281
1187.856779 2847.827156 985.553098 2787.561757
1485.040685 2897.213383 1266.373599 2844.271095
3012.161613 3501.609323 2541.957889 3486.105010
5266.122817 2917.711349 5042.698119 3009.327609
4196.486581 2763.796515 3939.579603 2807.032418
4520.593653 3535.396457 4092.257093 3649.398629
3575.942197 3768.184020 3025.241629 3803.332158
2014.431948 3783.239482 1558.538375 3671.330203
2900.854146 3166.777779 2522.513883 3154.563961
1518.292011 2505.465925 1344.014591 2470.345091
mrcal-2.5/doc/data/figueroa-overpass-looking-S/features.imgany.scale0.6.vnl 0000664 0000000 0000000 00000026415 15123677724 0026617 0 ustar 00root root 0000000 0000000 # ID Feat1x Feat1y Feat2x Feat2y Corner1 Corr12 PkWidth Pln Reproj
2 2874.000 2514.000 2692.879 2515.956 15172 0.984 4.2 0 0.0347
4 2841.000 2515.000 2659.755 2516.678 12232 0.985 4.26 0 0.0368
6 3331.000 2691.000 3096.094 2695.828 10919 0.982 3.37 0 0.1
7 3406.000 2818.000 3134.504 2822.231 10698 0.969 3.32 0 0.274
9 4596.000 2587.000 4318.438 2600.560 9801 0.962 2.98 0 0.338
10 2638.000 2521.000 2454.576 2520.959 9706 0.981 4.74 0 0.0699
11 3508.000 2837.000 3229.332 2841.298 9610 0.981 4.02 0 0.312
12 2652.000 2492.000 2476.968 2491.978 9576 0.976 4.8 0 0.0195
13 3938.000 2458.000 3744.325 2470.967 9253 0.899 3.69 0 0.859
14 3529.000 2779.000 3266.512 2783.832 8923 0.985 4.29 0 0.288
15 3323.000 2726.000 3078.756 2730.855 8814 0.980 3.88 0 0.0536
18 3442.000 2713.000 3199.203 2718.146 8487 0.980 3.37 0 0.161
20 3465.000 2690.000 3228.516 2695.323 8403 0.985 4.21 0 0.224
22 2198.000 2380.000 2044.335 2374.320 8155 0.977 4.65 0 0.192
23 2726.000 3047.000 2391.308 3051.771 7792 0.982 4.57 0 0.0664
24 2710.000 2483.000 2537.713 2483.279 7510 0.976 4.81 0 0.0923
25 2663.000 3046.000 2328.005 3051.142 7119 0.979 5.17 0 0.252
26 3615.000 2715.000 3368.866 2720.780 6969 0.983 4.27 0 0.252
27 2770.000 3293.000 2364.806 3299.912 6954 0.974 6.15 0 0.254
28 3492.000 2783.000 3228.646 2787.594 6717 0.981 4.62 0 0.349
30 3575.000 3281.000 3170.652 3282.374 6680 0.983 4.63 0 0.0516
31 4573.000 2667.000 4297.646 2678.268 6679 0.990 5.29 0 0.265
32 4871.000 2803.000 4541.188 2811.609 6675 0.988 5.38 0 0.0932
33 3620.000 3350.000 3195.409 3350.701 6613 0.980 4.8 0 0.0413
34 3989.000 2465.000 3783.698 2476.126 6597 0.943 5.73 0 0.722
35 3305.000 2532.000 3115.669 2537.324 6528 0.937 1.87 0 0.144
36 3356.000 2422.000 3197.401 2428.709 6490 0.969 4.31 0 0.103
37 3758.000 2689.000 3516.242 2695.640 6445 0.986 5.46 0 0.323
38 2652.000 3245.000 2259.455 3252.198 6194 0.984 5.07 0 0.263
40 3698.000 2811.000 3423.146 2816.002 5998 0.990 4.48 0 0.31
41 2622.000 3344.000 2200.615 3352.333 5885 0.984 6.03 0 0.153
42 3394.000 2711.000 3152.578 2716.140 5877 0.975 3.76 0 0.0193
43 2793.000 3043.000 2459.744 3047.685 5840 0.986 4.72 0 0.114
45 2091.000 2454.000 1915.600 2448.235 5684 0.966 3.55 0 0.04
47 2689.000 3096.000 2340.044 3101.285 5529 0.980 5.16 0 0.0634
48 3234.000 2350.000 3097.469 2355.766 5525 0.976 3.93 0 0.108
49 3787.000 2836.000 3503.081 2841.009 5498 0.988 4.93 0 0.252
51 3497.000 3284.000 3093.011 3285.632 5338 0.986 4.25 0 0.194
53 2015.000 2352.000 1866.045 2343.335 5189 0.892 2.19 0 0.0503
56 2318.000 3052.000 1975.953 3057.608 4724 0.981 6.28 0 0.0616
58 2383.000 3049.000 2043.085 3054.354 4719 0.973 5.75 0 0.00644
60 2655.000 3183.000 2280.750 3189.369 4623 0.986 6.11 0 0.14
61 2475.000 2557.000 2279.774 2556.207 4541 0.966 6.69 0 0.0782
62 2550.000 3423.000 2104.742 3432.761 4495 0.981 6.54 0 0.0445
63 2356.000 3123.000 1994.172 3129.564 4470 0.980 5.32 0 0.000137
64 2613.000 3090.000 2265.190 3095.437 4412 0.982 4.94 0 0.0634
66 2595.000 3145.000 2230.797 3151.224 4273 0.983 6.1 0 0.153
68 2768.000 3091.000 2420.975 3095.848 4193 0.945 3.86 0 0.0606
69 3059.000 2361.000 2921.043 2364.737 4178 0.958 2.82 0 0.0945
70 1851.000 2301.000 1694.738 2289.561 4046 0.972 4.43 0 0.0898
71 2455.000 2558.000 2259.298 2557.107 3976 0.971 6.42 0 0.109
75 2555.000 3049.000 2218.191 3054.159 3777 0.982 5.58 0 0.0984
77 2206.000 3390.000 1762.875 3401.757 3700 0.976 8.09 0 0.343
78 3586.000 2727.000 3336.942 2732.840 3693 0.951 4.76 0 0.0389
79 2560.000 2911.000 2263.306 2914.367 3676 0.977 3.26 0 0.0553
81 2694.000 3417.000 2253.001 3425.295 3623 0.979 6.71 0 0.0179
82 1787.000 3352.000 1340.353 3366.337 3585 0.980 6.85 0 0.375
83 2188.000 3053.000 1842.181 3058.941 3548 0.976 7.96 0 0.102
84 2751.000 2546.000 2560.810 2547.160 3469 0.977 6.73 0 0.00419
85 1673.000 3451.000 1191.367 3469.506 3461 0.934 5.2 0 0.228
86 2524.000 3159.000 2154.894 3165.647 3398 0.966 5.65 0 0.136
90 3839.000 2747.000 3578.934 2753.359 3247 0.987 4.21 0 0.235
91 1971.000 3058.000 1618.481 3064.322 3243 0.975 6.83 0 0.032
94 2817.000 3055.000 2480.911 3059.230 3151 0.984 8.91 0 0.237
95 3915.000 2836.000 3627.823 2841.409 3117 0.981 5.5 0 0.169
96 3844.000 2810.000 3565.918 2815.444 3092 0.971 4.42 0 0.283
97 4143.000 3288.000 3721.757 3285.631 3084 0.981 6.05 0 0.195
98 2605.000 2829.000 2332.338 2831.708 3068 0.979 5.41 0 0.0362
100 2139.000 3418.000 1685.249 3431.352 2988 0.985 6.58 0 0.0482
103 2491.000 3019.000 2161.710 3024.204 2883 0.984 5.21 0 0.317
105 2264.000 3099.000 1906.577 3105.461 2856 0.978 6.84 0 0.0101
108 2510.000 3215.000 2124.397 3222.260 2845 0.980 5.59 0 0.0115
110 2803.000 3125.000 2446.709 3130.120 2840 0.971 5.05 0 0.0251
111 2841.000 2367.000 2702.180 2368.183 2834 0.967 5.81 0 0.0213
112 2456.000 3341.000 2032.698 3350.678 2823 0.976 5.71 0 0.233
113 2417.000 3377.000 1982.765 3387.201 2822 0.929 3.33 0 0.00576
114 4435.000 3572.000 3924.064 3562.746 2817 0.983 5.86 0 0.644
118 1899.000 3417.000 1437.230 3432.227 2758 0.983 5.84 0 0.293
120 3696.000 2709.000 3450.112 2715.320 2724 0.972 4.62 0 0.173
121 2265.000 3293.000 1851.215 3303.079 2722 0.977 9.34 0 0.105
124 2617.000 3422.000 2173.407 3431.300 2639 0.987 6.89 0 0.158
125 2004.000 3419.000 1545.069 3433.690 2629 0.974 6.96 0 0.00696
127 3469.000 2553.000 3271.701 2559.572 2617 0.984 5.86 0 0.0658
129 2486.000 3418.000 2041.564 3427.566 2588 0.981 6.4 0 0.463
131 2469.000 2608.000 2259.015 2607.607 2579 0.973 5.23 0 0.0259
135 5714.000 2861.000 5268.772 2868.780 2507 0.978 5.74 0 0.227
136 2260.000 2844.000 1978.217 2846.129 2504 0.982 5.09 0 0.0356
137 2590.000 2472.000 2420.389 2471.272 2494 0.980 5.79 0 0.0391
138 2915.000 3212.000 2534.417 3217.298 2476 0.976 3.11 0 0.0956
139 5650.000 2942.000 5210.898 2946.663 2475 0.961 7.15 0 0.203
140 2044.000 3218.000 1645.882 3227.603 2467 0.964 8.05 0 0.0976
142 4056.000 3290.000 3636.998 3288.287 2450 0.985 4.74 0 0.0801
144 2262.000 3223.000 1869.045 3231.481 2370 0.989 5.14 0 0.138
145 1668.000 3251.000 1247.379 3263.215 2355 0.963 8.42 0 0.407
148 4228.000 3289.000 3803.175 3286.210 2311 0.975 5.29 0 0.113
153 2544.000 2257.000 2434.335 2254.113 2236 0.967 5.52 0 0.229
157 2076.000 3165.000 1695.004 3173.158 2203 0.951 6.4 0 0.189
158 4307.000 3360.000 3859.730 3355.644 2203 0.966 4.62 0 0.134
159 1966.000 3104.000 1599.762 3111.314 2203 0.979 7.15 0 0.102
160 1819.000 3103.000 1448.095 3110.667 2199 0.969 6.58 0 0.158
161 2442.000 2638.000 2222.992 2638.022 2197 0.971 6.74 0 0.134
166 1909.000 3060.000 1553.553 3066.642 2064 0.972 8.62 0 0.0651
168 4560.000 3528.000 4055.238 3518.591 2027 0.979 4.43 0 0.772
170 2178.000 3145.000 1805.315 3152.477 1984 0.971 5.25 0 0.0832
171 1943.000 3343.000 1504.283 3356.230 1976 0.976 7.54 0 0.0971
172 2776.000 3344.000 2356.399 3351.115 1973 0.987 4.85 0 0.121
173 3381.000 3107.000 3028.554 3110.931 1962 0.894 3.83 0 0.471
174 1943.000 3418.000 1482.625 3433.032 1958 0.979 8.04 0 0.158
176 3758.000 2516.000 3562.106 2525.069 1939 0.952 4.01 0 0.173
177 5655.000 2837.000 5224.062 2846.010 1930 0.979 8.13 0 0.0843
183 1882.000 3103.000 1513.811 3110.274 1876 0.935 8.02 0 0.299
185 2545.000 2489.000 2370.105 2487.961 1839 0.967 6.08 0 0.000272
189 2426.000 3049.000 2086.575 3054.504 1733 0.983 5.42 0 0.171
191 3939.000 2299.000 3777.023 2314.583 1726 0.880 6.19 0 0.7
192 2756.000 3420.000 2314.572 3427.788 1708 0.986 5.67 0 0.00325
195 1811.000 3064.000 1451.073 3070.735 1678 0.961 7.21 0 0.0982
198 3923.000 2684.000 3678.256 2691.595 1667 0.965 4.71 0 0.274
199 3494.000 2305.000 3361.856 2314.277 1659 0.916 3.89 0 0.2
202 1771.000 2240.000 1621.437 2225.788 1655 0.967 3.74 0 0.168
205 3993.000 2319.000 3822.322 2335.238 1595 0.888 5.75 0 0.966
206 2574.000 3347.000 2150.958 3355.700 1574 0.987 8.66 0 0.128
207 3739.000 2933.000 3428.607 2936.667 1569 0.947 6.76 0 0.34
208 4505.000 3576.000 3989.656 3566.177 1557 0.963 5.36 0 0.861
209 2104.000 3342.000 1671.417 3353.940 1548 0.975 9.16 0 0.102
212 2326.000 3187.000 1945.466 3194.460 1519 0.882 3.19 0 0.193
213 1596.000 3421.000 1120.058 3439.329 1504 0.964 8.44 0 0.232
214 2667.000 2386.000 2522.650 2385.169 1498 0.975 5.59 0 0.103
216 2906.000 3165.000 2538.565 3170.175 1492 0.951 4.04 0 0.159
220 2960.000 2583.000 2758.696 2585.760 1431 0.943 3.72 0 0.0765
223 4635.000 3908.000 4021.444 3887.721 1396 0.902 6.55 0 0.309
225 1769.000 3107.000 1394.629 3115.402 1378 0.954 7.88 0 0.171
228 2411.000 2348.000 2275.024 2343.905 1333 0.959 7.1 0 0.0899
233 2056.000 3345.000 1621.167 3356.885 1284 0.988 8.61 0 0.452
236 2882.000 2283.000 2767.199 2284.349 1216 0.956 4.34 0 0.0285
237 3345.000 2914.000 3047.246 2917.707 1216 0.930 3.24 0 0.253
242 1628.000 2346.000 1460.264 2332.672 1186 0.973 5.15 0 0.0288
248 3652.000 3347.000 3237.653 3347.780 1136 0.914 8 0 0.128
249 2739.000 3257.000 2343.795 3264.537 1131 0.951 7.77 0 0.776
253 2035.000 2349.000 1888.264 2340.119 1104 0.889 2.67 0 0.325
255 3227.000 2556.000 3031.747 2560.389 1099 0.936 3.02 0 0.314
259 3814.000 2596.000 3597.096 2604.389 1076 0.945 3.79 0 0.178
260 2749.000 2289.000 2633.166 2288.691 1075 0.959 5.09 0 0.00513
265 3350.000 2197.000 3251.225 2205.336 1034 0.879 3.31 0 0.274
266 4369.000 2424.000 4147.130 2440.084 1021 0.892 2.45 0 0.369
273 1625.000 2295.000 1458.996 2281.076 993 0.926 9.16 0 0.519
276 5113.000 2870.000 4739.461 2877.096 981 0.982 5.66 0 0.142
282 4347.000 2992.000 3999.776 2994.521 943 0.859 3.63 0 0.4
286 2879.000 3282.000 2477.722 3287.917 933 0.921 5.96 0 0.107
289 1733.000 3195.000 1331.353 3205.189 915 0.976 9.65 0 0.413
290 2300.000 2310.000 2168.420 2304.304 912 0.951 4.71 0 0.0557
292 3409.000 3213.000 3026.285 3215.238 908 0.923 3.38 0 0.35
297 1733.000 3344.000 1286.795 3358.417 887 0.971 7.42 0 0.469
298 4612.000 3571.000 4093.391 3559.716 887 0.926 3.67 0 0.575
303 2913.000 2220.000 2816.222 2221.589 849 0.948 5.89 0 0.0585
308 4274.000 3555.000 3773.511 3547.086 840 0.946 4.89 0 0.102
311 2983.000 2421.000 2827.995 2423.995 835 0.933 9.38 0 0.133
319 4470.000 2427.000 4244.407 2444.124 792 0.900 2.74 0 0.249
322 2348.000 2876.000 2058.433 2878.876 785 0.938 4.32 0 0.0908
334 4400.000 3363.000 3947.366 3357.614 723 0.959 7.43 0 0.0616
341 2864.000 3374.000 2436.313 3380.384 712 0.941 8.39 0 0.067
350 2987.000 2375.000 2845.281 2377.822 676 0.918 3.45 0 0.0133
351 2955.000 2622.000 2742.944 2624.777 670 0.948 9.27 0 0.102
355 4884.000 2629.000 4597.668 2643.479 655 0.955 5.19 0 0.00882
359 3709.000 3561.000 3223.130 3560.207 648 0.970 5.57 0 0.769
360 2831.000 3615.000 2333.866 3624.609 646 0.911 9.58 0 0.659
361 2517.000 2879.000 2228.975 2882.067 645 0.895 2.53 0 0.0251
362 3406.000 3186.000 3031.001 3187.799 645 0.890 7.52 0 0.749
366 4339.000 3295.000 3908.576 3291.977 623 0.974 7.8 0 0.283
367 3660.000 2465.000 3481.428 2473.990 619 0.939 6.37 0 0.076
369 1544.000 2701.000 1276.406 2696.991 611 0.968 4.51 0 0.0888
371 1497.000 2703.000 1211.528 2698.953 607 0.951 4.24 0 0.0184
378 1845.000 2684.000 1600.843 2681.219 593 0.952 8.56 0 0.00907
379 3010.000 3064.000 2672.000 3067.716 588 0.937 3.93 0 0.351
383 5317.000 2938.000 4909.054 2942.686 587 0.987 5.54 0 0.15
385 3833.000 2530.000 3630.512 2539.334 580 0.956 3.69 0 0.273
389 4345.000 3357.000 3896.983 3352.598 573 0.959 4.79 0 0.255
397 3454.000 3390.000 3020.585 3392.031 539 0.928 8.67 0 0.236
400 2172.000 3499.000 1695.552 3512.586 536 0.863 5.08 0 0.845
401 2936.000 2771.000 2680.959 2774.295 535 0.956 9.85 0 0.0335
406 1932.000 2364.000 1752.355 2353.978 514 0.892 3.83 0 0.616
mrcal-2.5/doc/data/figueroa-overpass-looking-S/homography.initial.scale0.6.txt 0000664 0000000 0000000 00000000224 15123677724 0027331 0 ustar 00root root 0000000 0000000 1.06780825e+00 -3.11887689e-01 4.85218463e+02 4.84659653e-02 1.01030843e+00 -9.34182334e+01 1.61828748e-05 -7.38019356e-06 1.00000000e+00 6016 4016
mrcal-2.5/doc/data/figueroa-overpass-looking-S/opencv8-0.cameramodel 0000664 0000000 0000000 00001331007 15123677724 0025372 0 ustar 00root root 0000000 0000000 # generated on 2020-11-14 21:49:59 with /home/dima/jpl/deltapose-lite/calibrate-extrinsics --skip-outlier-rejection --correspondences /proc/self/fd/15 --regularization t --seedrt01 0 0 0 2.1335999999999999 0 0 --cam0pose identity --observed-pixel-uncertainty 1 data/board/opencv8.cameramodel data/board/opencv8.cameramodel
# # # Npoints_all Npoints_ranged Noutliers rms_err rms_err_reprojectiononly expected_err_yaw__rad range_uncertainty_1000m rms_err_optimization_range rms_err_range
# # 81 0 0 0.5864956382518457 0.5972518353763385 0.0006951826725578303 325.84567147387764 - -
# # ## WARNING: the range uncertainty of target at 1000m is 325.8. This is almost certainly too high. This solve is not reliable
{
'lensmodel': 'LENSMODEL_OPENCV8',
# intrinsics are fx,fy,cx,cy,distortion0,distortion1,....
'intrinsics': [ 2073.872915, 2077.452267, 3004.686823, 1997.377253, 0.4791613797, 0.0266824914, 4.398264387e-05, -1.180073913e-05, 8.959722542e-05, 0.7666912469, 0.09633561231, 0.001407513313,],
'valid_intrinsics_region': [
[ 622, 0 ],
[ 0, 634 ],
[ 0, 2747 ],
[ 207, 2958 ],
[ 207, 3804 ],
[ 415, 3804 ],
[ 622, 4015 ],
[ 830, 3804 ],
[ 1037, 4015 ],
[ 1244, 3804 ],
[ 1452, 3804 ],
[ 1659, 4015 ],
[ 1867, 3804 ],
[ 2696, 3804 ],
[ 2904, 4015 ],
[ 3111, 3804 ],
[ 4563, 3804 ],
[ 4771, 4015 ],
[ 5185, 4015 ],
[ 5185, 3804 ],
[ 5600, 3381 ],
[ 5808, 3592 ],
[ 5808, 2536 ],
[ 6015, 2324 ],
[ 6015, 2113 ],
[ 5808, 1902 ],
[ 6015, 1691 ],
[ 5808, 1479 ],
[ 5808, 423 ],
[ 5600, 211 ],
[ 5600, 0 ],
[ 5393, 211 ],
[ 5185, 0 ],
[ 2282, 0 ],
[ 2074, 211 ],
[ 1867, 211 ],
[ 1659, 0 ],
[ 1244, 0 ],
[ 1244, 211 ],
[ 1037, 423 ],
[ 622, 423 ],
[ 415, 211 ],
[ 622, 0 ],
],
# extrinsics are rt_fromref
'extrinsics': [ 0, 0, 0, 0, 0, 0,],
'imagersize': [ 6016, 4016,],
'icam_intrinsics': 0,
# The optimization inputs contain all the data used to compute this model.
# This contains ALL the observations for ALL the cameras in the solve. The uses of
# this are to be able to compute projection uncertainties, to visualize the
# calibration-time geometry and to re-run the original optimization for
# diagnostics. This is a big chunk of data that isn't useful for humans to
# interpret, so it's stored in binary, compressed, and encoded to ascii in
# base-85. Modifying the intrinsics of the model invalidates the optimization
# inputs: the optimum implied by the inputs would no longer match the stored
# parameters. Modifying the extrinsics is OK, however: if we move the camera
# elsewhere, the original solve can still be used to represent the camera-relative
# projection uncertainties
'optimization_inputs': b'P)h>@6aWAK2mk;8AprP8D`%|$007_s000gE6aZ;%baH8Kb7^C9E^csn0RRvH-~a#s00000tpET300000o9q|r8xYCJP{vTLo|0OeT%>NLpl*|9p{}E#o|a!!Qk0k%pI?-c3KDlq%qdO?A9+gBDe+f?p7lRw-Z08mQ<1QY-O00;m803iTcY7!NLpl*|9p{}E#o|a!!Qk0k%pI?-c3KDlq%qdOp^Rf@bEoTGT=UxF+OvFo|Ap`Cx7T@`$9bIbc%J9;gk89DUf*~XhbxEI0V``~OQ!>>Vh4`fs2q?HJ79Cy$;HXs(d@31mG%GGwap!ztyk>McINl2SN@U;Do13*j!4Ofd5ir&AMdfw)zfEWyTDvaBFH9o8sz4F)Oj)+;JvxmxG-m-UV!=bybW&a?h%1IXMF(?OR0qDPO?i*&HF$qX{&q4Bu>GD^`p7`CVZ5!7R0eP9-wn(a7G6$uBdoXWG+eydedzb%dHorPJWqoRkUs5FHKtSR34%`oT-(Tuj?j8f*~To#0qA1D>6Qg4=f%V?w`WFBdNzkYJR3;2Z^wiKzzOYX_l{R(@xIp9&8PYp)N_&p>gLhNi}aN{sjzIiBp!fJ@V#8v4QsnB!wgN~#)!t@1y+L%tEgX}s2Qt;Q_Wg#3D%5=X#;!h8BQHnsxKq}bPenw9uUv-^Cg<`5Kqnc7`VZ-oCIoe>LOnt@H$|FriHlaV7qZ6MmX6=dZ|$-c~DEK0h%3P?k6gzJv#p44A3^3mBt^3XJR-nnRG`?wQJcelUM|3Qbi>#19Fk{uROql*_S+?JS0DlXQe>0XIg>vWMSPjKIaPuRC?3HH2kzJoV&)ark<1H##``IJV{abVXtTh472c1{=r#A{qVIqAr-&`n{eb3xBH3SN}y2DyxU6^_>My;%?3^%aM`CMv9(0nc7%f&sTpl3&4av3E;+Lsf>+4sH!d2+AQZ@XsHr>(ZtGM_->myvs`XmxP1;AzzM&n>{o@560wPlg-sB7P5Shz0M}3%mqvDmHFyOW4>vff{RCjDIYafz!~Nh>rzyI5_B5COe;oT5{`-|8dSo&I_6Eq9kkK^XrB^+?UzNS7kC&Uet*BuHk;GPB($b+1+Zj9q~A5`SeG3J)$U6q?&TN4lcY^(VcQdBH~?C6|XdwvI=BQW4R`to7jw#d@eB=>BJSn~k1}ZC8|Z$avta&6>SmDbRD^S$L7ncSyW-FaMEg5o%nCIcXJK57d-L`56{$lzLa9v(Kmr1s^}uts5tUVfeTinUn+W%@Mp|?L~OmfBh%V*Y)7&oc;UON_^+<@|RtzX;?FM%5v%|8ICOzPUET~*q%LluD}&>-*sSIZ?tDJCvlVSV@#qa7Bf|HMObeD_EhusDahysiMj>bK9Ud)>u>aMJ{-f(`+zTT!Xrk2l%oq4q2xwTrseE7%tpL@_KL^)KMlaA)V)dI5*ydoByvTMGqHJJe&I!tMyS#&i_~Wl;gb1CB71i+p54x4=I+q|4~nD}g$&u4b3t{yd7vE~-&`JiySg4M8A2t7YGn9e>R@~IMG2;>S-AbkZUmi^&ALWLZ2bPFsQ+I?2kMI?*}rVBhFr5*{XQ)!q!qnti{4d&6{=DdyrzwiMk_wN-++x(snub7csj8rRk0>duL^dF*XtO{(7>zi$EBqsB-wcC>b%~AOI>I&_d!Waqa2)CMA+H=bQr#+^WHbD7;QyIKL@*!z#&uc#k<@wEX$>Dj&<)wGM~EcQL8f8HmYR%pFSOuu919L%7wUwcE{f}i40Yn%E}!j$8h|}77w?@ZZw-c7qdqH4;)kq{=4xh9qbnEk1~@pu}F8Lc<&uD)W&N`@tcm~hL|_no4I?C8OxozHKh*bO!dNfN9X{KLH_EkVTfCNO?Zxwfp^y5`~BGwykzKkC+<)W4xc*mAm%m^ik(D)+O{*GW>?)LH6|9yY`F53K9itofa`UF$S`gq+#7u;(1V4RK)})-;tAVYNIvUIrL4pcmy`>%&+J8$sx?Q4(t40OF}dCC1{3a|?;6=W%z$mf3Fo1L085cFGfuZjFq5F6_}7|+)fo+EPu%Oln5$vSL-#vi@Fne*b9xJ~pKB1Y%q8z`k>&3&aH{Si+(gVt>&LR;}OyDX#E<3%Z8}7a#2WL2D13T@@WVlE?&2m&fNdH?UxhRdkNjeG-Vf&a&>MG+_Djz}zJw3@N-`)&i!OrdsQGp-r;B;4H-@a9SaEFnaKsF?R+{k6C$+${fC0hRXQ926?-rjOFSY(0s+IIml1|3jo+X7O8128{QH^i~I0nQN?449nDFr#FM&Pu3h-$q}9rUhVcu6(+Jg`?X_PDD8G)e*SPT@Hl@u@VcP`P9EMax6ynIvs)r)M@Qf|QMi}3*jjLjYg}bJl3{SQ6!0LY0_|%k62yy0U(h{G5>B3K;GSBKE>v8jkI3fwGO^nXQAL>PR$<@VA$~_Rx8SmxH(FK(e`8yh60zL~bcoKUXp|wZyb@eP6Hk{K9{IP-qfe(B*K0d0C)*J?;cqVZN9`{?yFTM%lr@`##KsvPX0)Ppu_ofM;QIk)DkjF37NxZ4=+K={N!-Zn8k}GA!~YNC}@_OD(6CKMII8bp`te#84Y?1A31a3e5anSQBXhJwa|FM?>K`2jSq%rq^Kyp_2Y0?atrS3WvUS8R`The?^d4q4g9b32*t~N5Z(6Ao-cM94fWOvYL+!&;J@!=1vUdIq2mR=2;>!AAgJNaT@0kw0{6+(KG^!-Kf(1GQ|Lq(3ohPH$M+Lj2OOjrsQ%Jj+N+<8q*1{`M-G<)pXA1|Ggtb+BI~`dc;h5C3{>)T`Z17D>9xoDBpsDhM>qp`C}>}%$)~x$9JKCxnCsj1f|+gB*PgTq%u7?BRa$Jp7Sq5DSPUz{W(dRVOAH%ms}$$`KyQXPm1T~1rfV!;gW`|umQBWMu)U%Q7s6F)EBok}kw;@Az1!k~sZi;PbLc1K+J+6Yt6x&G$s_Ck&P!WYNnJ`_1P{fobOWv*+H!m+Um>zQ@0?@`H^quy-zAgvJudrkOu$@apz4!PeGk}Tw!qR&|EZ9yj&FxaqMf-W&Z>r9T5u|+Il=uAW-2=%`Rs?1@*CNas-+(+HGR^?r;qHqhwemZC9r%`~}^h;|cACpjpIr`$}L?d+Tzk2U?k_8O5T&`MRC-z?KW={NR!MQu7p#oajcv`G*jq5rR#>`*08xkeJvkYSJ*OfigBtl?ad(K3oo~v6Zb*sxoO5X%6|Js{=IAT6VTvNR5K7zzs@;j8b!P~;#XZz?jM4zyuV$o1wspfpy{wFVYgg{FE-4dxwKnW9RV-$V!~@+or~d&z0n=sO{%S_n!|)bS^_xQ7Abd&ogZlMKoJ-ev$PH~sY&Uw`{xb`lJ}rcZaS`x)lSNy>lUnd?`DD1+p#uZd8*7X&%p=eCoqJCyRX}2_7lHkYiH%kwo7v?nc$f(_CpA*RHfn4n>4iI&9S4Zg}(DxXe$Pf{gQD~EZIFRzZ0#L?=WijF;Mwq<|?(SIv{JzefaN2D`vV)3vCye$Fl#FKTatA!k|}y3)0+OXt$%v^NcDT|4P=^^Jvt9`h_2T7X(_c_w*D0-5GOuN=9afXh2!Jb5zm21yzZu0{5OfsEtZEu=IJ2Qk~dhna%Ya(EWHX8Ct={BxD;XG4)`gs%hT78i6S*cTCZFtkTpaQ?K!-nr&FA%JGm%GN}B9j_k@VsEO|fme(!+JMwaO{H74GO4~Q|W7{`ZHMb5iFTH$ouDO-y)A`-2t59cfZ;pBzX(Zng3oFB+Qj8n-1%xMApg)fwwGR;+kZ*Ywd{naiyt+Y2+I~F>QfhFj%sfhUP{F=u~n{HTpCc8o%7tkq6@5CWF1E|X}CfB^mVPyezfoj=#o?KhH<{@iq0j@xH&;=dwX>~+Q~I_*igCvq>@=P(NsJ~ZjDkn)`yc)@)AEYy5UO|XXR15cu+Zb!+C&JkJCQKd7|^X!1wJ`;A>wB7VY3k+wqKrMxlGG=0m!nTa&JMb|D+CzdGZ7!@2KV0X?)wlloYlaMS0)A)eJlWYs0!*=g2+qbJn9hvjua{8>Tw@0r!m+@nzt;77okrk&G!LOWrBc6g5qwHgOBwPPwoNm#4(R3cug1L|%;(WoI6`W~Df;4vlRtNqse!6r;dpLxFF>)JoK_R)|&=hmu~Jvk$2$JY-0{lV`zR?~r7`mMr7R|oYIK7(gL+#4e$Fc7UQ^!0`K|k-zfIvybom^6Xu5AFC3Cmcu4^AXvb`1tq*xZX4X~yg09qH#Z3AiW7@=~L8GTaoIJ`iX&1|QVy3IdfE@%6&zt9yOxa9LmUirhF2h2Ke?PK+a`>7YUyipg!4I$l9W?gcY#1v&w51qd^_gUdi_Jn4~B?l36ccJT3?pGLmcGuqG3Fi`y5*}wb_BuqPL?eEu2K;ML!7xd~1s4LyO!!LdaPIYyt{d+r!#{>5te!apU2jta$SVq<3Q~DNaItLL;CXUOeiPXUK1`efN9|mFb-K>^+fiZO1wl;I9n2GItcCu#th~K_`Zg6@|#KA5#1;(X%kg;5QC^E1I^Op&%pzWWO7eJ*u(?#(13Lg{+GzDw*f>>U~RPU2&pskwp$-K@6OOK&nJH@-v^Vr+-;m_*z+pCh+Tvq&mz>Foy+mPkII!x;)9?$L-=`W;W3!5?d~C3j)BbUA9@s{c4ToEnbZAt=#qI#rt`mbS{EW)ab4-iA>NTIKEmK8-kRpF`Wzb73gz8x5^}S1PUu#eW`j3^gWp`CQ-WxAwT$>Xxhb~ZoA7Nd(|*xRaoWSZEC=2F-CNh^a$*bE@#E>qv3M9WaxCoB9OKoEVA5I36XDq2&u^oLwkB^%`poK&i>l+?XJ@>xYAELefUX5##NU+yh$td5;Cqdd~bv-<+W~>MZ>UgXXM_Mm3Rc5C!1_n>N#6*lpSYKi+=rm+T_N0_^@n#LR_K^cJDd6(b;1N4i#GLsN(BF_30h+-^B)jF-PhvaIC->tN)gaug=4B7hPSh6`m3_9WHSlAA}6g;Ji|i9yH!xEueq{gc>s*Ihvi6{=3z~Xe&NfP(;!pz_xmZhulvNUSg9A>OV25th+EP5!u#uFdIwP@IwM?5bO6S0UpFv%5stpb28Xk#laLpkf4DHH7lh>}N}gQmfxG_iO@(WRv4GdS+198Zguc8uu|_!wgTFN&btkalRO+UU37>o64VB-vP`edeX4EfaD378fS3T!5u0e3iF*(YADjmfZ=!j`at#5QfwHt1y0GFDzh^k#lvw2w_0u-gt>;{rJ|N^_$ou-pZxeJ4Ex=>*}1(R(i}yvs6C*=x`x>SQ_(RDo~wO3%^ZMKfmAHi*3ngbG_t~rn+9LZ_Q6r58<%p9HNne<)uX8?V^~~%cBdQvNnOmyHmWT_>>zQo$GIdN^+JcVSg7z2GAI<{V);2UQBLvR-cW0V2+JIq^5npIig)();1IY)+Vb3+k?41tH$epaPH6eZ$Oml9(`_}OlW`ZTOUao0{s=yjO1)W-S-cS3rU1}T*Iud*NG@5aX@bJg6uvuvGJwIq76$i~TYhT{Tr9(rpxcMEgTAa3)wK#lX9E1}~Jlt3GuD_FCN{_`XP{w1^4BBFWd#>QM%g-kG>rjwy?Olh8VqHo5{Kp}mlPS_yM8ikF{>$6Ca~Ag86}o(I#ov>+t#-|8GQ`c@7z({#hdNu!H=e2)hw0gGl@dE>$P-2yT-`APNnUTWQ&W5^&E{y|qJ$FE0PdI+#E#K=p-47QZq@TngKYZ7<^MqPlDQ+h2Z&r+;;AWRQU1f^>V9Gh~wN&>GMa|oBuC5t4aXUU;(E{L{;S{t#99HEg=*4?f=a^g#D^DpozJ-8@y$2Om{L%CCvN!4Y0hmlUNQh)h1$5x64+iXti852b7I`hyc}X-g`el$O6mtnLNbgwtg_g*P#~;aX&}Y$tdmn!CK?af4IOOsoC{tmrl$=Z{+xsd#^mliR0`1%qECwOOK;z^%H=)b)HDSQzM;M9JKP(|djyi`f$pCtrL`Eu&(kg#0cM0~TyMpcU|)@f?mlxk=_E(?L_4Ywt_eW3)cDFUK>Uh+?ptN*$)6XJ+nZeU}~}IqpsFH;M)wRg<;dR!wl+!i_9b7>G`j^}}Q@680>QRZiqJq3ysng+)RaWH7sR6D^Y9>%+bFCs*e4Q>$LIkmiK6A%crYG#P*N%)ZwcZbIVw=l6&IbUXW-S7v3jgcH_lhEL)}x`YYaZp7A26V7-nnc(D-SYrhJ;nj2&kO~zk10O#j&^Q_8-n=(+dT^5Z~n&p_UTj%R6g5B>I9$HLin;)T-uY1olH&SSZy8AVS!Ua&N#;&$KEet~@r(6@#tuYb1!2*K&D?+5yDkDrW0q)|Gq+5Gj8nG79y>WVH>eW^HL-K=zXctw}Ruk!AhsRWg>mz$1U8^ZJhmbd>K%*9i$70xC0GjIc+;sx&zD&`7$3xBy<4hOV6c@v_lU}#P1H*)PLI{wp@MRAM3srtI!T6rXM$;qMari?`fCdHZ_JqnE`^v<(v`hCB$4U6g=WvWRl7Q1yO?`@0j;WbD5^i?b8H(G?9QGfk#m^1Cs5$lU?BkqX9h1?myjVbpxBc7v=-0C6H?^0M{r4RA=pEQ3TZckblllY?EF>xH94~m4~i{FADtwFbL7b2iV8+m!?Mt3%OTXe>3SvUbtBN}6|P(K+OU%A=)`A5Xs0;#!#)El3JWxq;934ZWCuA`uymS0p-l$aNvUzCyx5_e0?DNY577iT0EqyqUGnmP)#3KRh@1|R?cP)h>@6aWAK2mk;8Aprd-3vzW(1OPzL1^^ZS6aa5xb7gXNVRUJ4ZgXE^Z((v|E^csn0RRvHK+pyN00000bx;HV00000l%07zRp0mjladBf3KbG%s$@ut%Bob9v8YHQq(mZ0NRkqTkYtW1^3K=!kfB$GGt&lk@^Z)rXKX+Jne-G=VAuI**CwVXmp2HdvA&j2;kLj#RX+z0E&5*eOz8u>IX8xEO@9lR9mFEn|qvFS#>MhMDjt*xN1yoXg4?hOmt&f^JEIh`2X=0Q9RF$f9PL){6s4SBpF=>G9dw6K20I*;J^pz`*XQvddTT(TWN5xtKe6f!&xq;U)W)Nav>e=IOs%X=m-o9e%W0-I{uYi@nL#MNW=xwNu+G8Mx2T`^fwgj~o%EoR{q0^U46@U#35@}Y~@21XJH2&akKJV{5cx8|D42O}ChNzM26et|rwX5W($DLT9t`Y5(cGMLN7KDWx=n8Sd<`RNOCp8v<2bX{AQ{VgZ2?}?3GxpTi>=JIb+ciUHcvS7w-%X)u$DEBx=-Z(ltH?u*hQ%h9TF^tO#)77k|t)+m}=^fLqgkR-y-{QOLmWWZopw6)H_h;l;!ACq9GYF`pcQ0OUh{t)EXOK@zCLq1BPkihgq+idQQKK;PSteH;GxJbch&VaU+U)joU7*r$WCB1(|R>#QKET!$@v?UV(`PpEe6x(ya7WD@JqUh0IGHe+y%SxBO~@iWxCn9-y$&I(!=iEN@GvofpF6@HMQkNtdQVr_N)=C9cS8!@o*ym`Q-Ed)S-RUysJZqij(}Huf$J7}MsazUT4a^pMj8Z`=AZttJete>{5GHZK1u__kflDBqSv{6`OPf+_<``!9$V_{*tdEEndZQ+uJvLzYo)h0wK1N);k`do}z;lH*(4L=3TTrQfiEPRJSdiaa4c+zT4#qw?~nGf8(D%FPwlb@vci@<*4&?>V4zeN;I%32)%l|6m?!Idwk>J2O7-yr5Uw!B$mrPzJR`SIvw6By?DcRi{o;M&96trFEe1DjL)t+8VOvUIInGke-IODHqh_K?@i+J?Hj4y-LWh<9QehW@g;@JSLg*7Z8D|+L(eHL#uW8zW6;mOC-3K;Ki<>_La3*H`l{k}zo~G??$>4SPUM%5cuwo{BVd;2kD2Y=*v|!{%?0wpGI`3zlZxa=yLd98+d8!%M&-TjWK~Ec~V0w4`gE`r#KVq!||5+yj@_QedOTI__f0vB^O!`59?w_hOV=dJGv-*3E(?uGLU7a~Cts@w;7?1wq7v=GOCIrk|;-z!!BI>UiPYj$_L4djRC&A`PsQ(`2=A6xYXt1cyLF-{X^7}b^BjGs-kh>&R($azL(v0h42X19RWKuruj9faGmlg4?EZ`&Wb6@4V1^yWr?>Dn^n;HdL-uqm-rj7a?ymG!7Hc+7B$D6IqyHWo&;{(43d)D9^JAVo4OSi~Uemu6_0P`V^7Jc}9LK)qUj8nN*QNGLyYfmsI`lp4xRJ3Q+qE;|j*tIWI-Co#D^mBwc2#zq(ds6j_tIL8pbMpUxcu-1L-p*6JUH~wH=z3^`iBc+E-h7g6sUe66>yt!P|cHf^w1#(;>2k7BEumABTT@!)rFBxMrkeKeeF18STnvJ9D?peR-;ILmHF@x^Ljb9t2toc-i33}~mHSm2ZC`cyIOF!wl%QeOT!J2@Y0TO5s6^f>$%|JaEHLx1x?c}<&Fdi+Ihe1>EC=gzMb=&$^Gb;<7_E_cit@|WwR!hEgcYAO22O|KL`^^PE5uGoC{__zyP?yTm0(?gF2QeK|cuet)cy!_)F0r@t?|I_3|9F#b!n&obUy=-%C%3cByAQ)x>F>yY2uAI1MVmjGppv@RiO^dZChbY>?ZksBG*ez9Ff2lW>RfR=$`CRw|2<QT6S?q<_#{N^!!Csy(0yFX&s?cr`>Bdda|7cUi?!(FX%*_-^-lND3jw=P|~5ATXRuos8_b%-IsKjzEC4X*&Fkt&H~A0GieN18j$e(m=fj@B}?PYVRR;(UAk<;ELF@GFQe0s)t0ePI_xo&w2?sX_9AsOMC1Q@p(~74FIGzwvlI`VIbrm3jFK2`E}(o@FVHI%nywP4P)0^HIgAi?78|r+<^O$!c{Py!B7NynYMj@#w5Tv!Qw#^xIzf^g09e&u};;mU@E@17YpS&$pufC#!}A%8oFgN3zV+C>He(tq^RDG+_e2uVH#31M^);k#_hheHJX4(y}vPB#p~2Cl)5_22tSqX}e>4`ceP3s^f!S^r@h`cR%fMCF)lb%#~ayMg04{?LYl*b?GZ^{z?cVAbYEx+JZ6EfAhnxX+z>PNF2>6P1=R}mwX#P_l!=1JMoq~U%H|GT1lCkDK>PtU}lhX#2oeSzdF&%s{V!!Q4Hd4%ol9eY$XBxNx=-LoA;tCXg2zd4Ama@cms0&csFlO)&cYzq}}kS^`k)HMX%iqZtRIe$I26{egfv+U;|H?nM2161rjg571y%~5%f5@SLz(c!_lZVk)w7=i9Yz>79OZI-ZeESl8#gx3FYX^=KV0Vn&-yax==k2G91C4xLo(D3E}cr739xm8o1Rj`EytdXOkK<@Y2bcMq*W&w`Ro}xDxWCQ!TDl`uj?b^g{i&u$>g}yG7x(A^DpFEYp9-~YnZUz@^Rc{`~Q4PlP)|>(0t^_f(xGr*Yne(xV&3eUCOzG{qOgwSmzPG@OydgAquF)n!AdhLtnAfHzjvR=`$aB^`Q0o6ePsVBQbhTb|@7#sHlsM~lk~k$-LNu&$S5fluL_Tjbcxu;arwaddpZN;_