├── .coveragerc ├── .github └── workflows │ └── test.yml ├── .gitignore ├── .travis.yml ├── CHANGELOG.md ├── LICENSE.txt ├── README.md ├── docs ├── Makefile └── source │ ├── api_reference.rst │ ├── camera_model.rst │ ├── conf.py │ ├── emit_sample_pymvg_file.py │ ├── images │ ├── camera_model.png │ └── camera_model.svg │ ├── index.rst │ ├── plotting_utilities.rst │ ├── pymvg_camsystem_example.json │ ├── pymvg_file_format.rst │ └── pyplots │ └── plot_camsystem_example.py ├── examples ├── plot_camera_system.py ├── plot_cameras.py ├── ros │ ├── draw_rviz_frustum.py │ ├── publish_sample_cameras.py │ └── ros_rviz_config.vcg └── triangulate_point.py ├── pymvg ├── __init__.py ├── align.py ├── calibration.py ├── camera_model.py ├── extern │ ├── __init__.py │ ├── opencv │ │ ├── __init__.py │ │ └── npcv.py │ └── ros │ │ ├── __init__.py │ │ ├── ros_publisher.py │ │ └── rviz_utils.py ├── multi_camera_system.py ├── plot_utils.py ├── quaternions.py ├── ros_compat.py └── util.py ├── pyproject.toml └── tests ├── external ├── mcsc │ ├── __init__.py │ ├── mcsc_output_20130726 │ │ ├── Ce.dat │ │ ├── Cst.dat │ │ ├── IdMat.dat │ │ ├── Pmatrices.dat │ │ ├── Pst.dat │ │ ├── Re.dat │ │ ├── Res.dat │ │ ├── STDERR │ │ ├── STDOUT │ │ ├── Xe.dat │ │ ├── basename1.cal │ │ ├── basename1.rad │ │ ├── basename2.cal │ │ ├── basename2.rad │ │ ├── basename3.cal │ │ ├── basename3.rad │ │ ├── basename4.cal │ │ ├── basename4.rad │ │ ├── cam1.points4cal.dat │ │ ├── cam2.points4cal.dat │ │ ├── cam3.points4cal.dat │ │ ├── cam4.points4cal.dat │ │ ├── camera1.Pmat.cal │ │ ├── camera2.Pmat.cal │ │ ├── camera3.Pmat.cal │ │ ├── camera4.Pmat.cal │ │ ├── camera_order.txt │ │ ├── multicamselfcal.cfg │ │ ├── original_cam_centers.dat │ │ └── points.dat │ └── test_mcsc.py ├── opencv │ ├── __init__.py │ └── test_versus_opencv.py └── ros │ ├── __init__.py │ └── test_full_ros_pipeline.py ├── fill_polygon.py ├── pymvg_camsystem_example.json ├── roscal.yaml ├── skew_pixels.json ├── skew_pixels_no_distortion.json ├── synthetic.json ├── test_align.py ├── test_camera_model.py ├── test_dlt.py ├── test_first_principles.py ├── test_multi_camera_system.py └── utils.py /.coveragerc: -------------------------------------------------------------------------------- 1 | # Configuration file for coverage.py test coverage tool. 2 | 3 | [run] 4 | branch = True 5 | source = pymvg 6 | -------------------------------------------------------------------------------- /.github/workflows/test.yml: -------------------------------------------------------------------------------- 1 | name: test 2 | 3 | on: 4 | push: 5 | branches: ["**"] 6 | pull_request: 7 | branches: ["**"] 8 | schedule: 9 | # At 23:25 on Thursday. 10 | - cron: "25 23 * * 4" 11 | 12 | jobs: 13 | test: 14 | runs-on: ${{ matrix.os }} 15 | strategy: 16 | matrix: 17 | os: [ubuntu-20.04, ubuntu-latest] 18 | python-version: [ '3.8', '3.x' ] 19 | steps: 20 | - uses: actions/checkout@v3 21 | - name: Setup python 22 | uses: actions/setup-python@v4 23 | with: 24 | python-version: ${{ matrix.python-version }} 25 | - name: Install Python package 26 | run: pip install . -v 27 | - name: Install pytest testing framework 28 | run: pip install pytest 29 | - name: Test pymvg 30 | run: pytest 31 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | pymvg.egg-info 3 | dist 4 | .coverage 5 | __pycache__ 6 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # http://travis-ci.org/#!/strawlab/pymvg 2 | language: python 3 | python: 4 | - "2.7_with_system_site_packages" 5 | - "3.4" 6 | - "3.6" 7 | sudo: false 8 | addons: 9 | apt: 10 | packages: 11 | - python-opencv python-tk python3-tk 12 | install: 13 | - pip install -r requirements.txt 14 | - pip install nose-exclude # for --exclude-dir option in call to nosetests 15 | - pip install coveralls # for coverage testing 16 | - python setup.py install 17 | script: 18 | - nosetests --with-doctest --with-coverage --cover-package=pymvg --exclude-dir=test/external --exclude-dir=extern --where=pymvg 19 | - if [[ $TRAVIS_PYTHON_VERSION == '2.'* ]]; then nosetests --where=pymvg/test/external/opencv; fi 20 | after_success: 21 | - coveralls 22 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # v2.1.0 (unreleased) 2 | --------------------- 3 | 4 | ## Improvements 5 | 6 | - Test against Python 3.4 using Travis 7 | - Build with `pyproject.toml` and `setuptools`, removing old `setup.py`. 8 | - Convert testing library to `pytest` (from `nose`). 9 | 10 | ## API changes 11 | 12 | - Properties on `CameraModel` (e.g. `cam.M`) are deprecated and will 13 | be removed in a future release. 14 | 15 | - Intrinsic parameter matrix is normalized upon loading in 16 | `CameraModel._from_parts`, which is called by most constructors. 17 | 18 | - If an input calibration has a non-normalized P matrix and a 19 | rectification matrix, a warning is given as this case is not well 20 | tested and the behavior should be considered undefined. 21 | 22 | ## Bugfixes 23 | 24 | - Mirroring and flipping cameras with skew, distortion, and 25 | rectification work now. 26 | 27 | 2.0.0 28 | ----- 29 | 30 | First real release 31 | -------------------------------------------------------------------------------- /LICENSE.txt: -------------------------------------------------------------------------------- 1 | pymvg 2 | ===== 3 | 4 | With the exception of the other parts listed below, the source code is 5 | licensed under the MIT license, copyright (c) 2012-2013, Andrew Straw: 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in 15 | all copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN 23 | THE SOFTWARE. 24 | 25 | other parts 26 | =========== 27 | 28 | The pymvg package includes the following other parts. 29 | 30 | _fill_polygon() in test/fill_polygon.py 31 | --------------------------------------- 32 | 33 | Downloaded on 17 May, 2012 from 34 | https://raw.github.com/luispedro/mahotas/master/mahotas/polygon.py 35 | 36 | Author: Luis Pedro Coelho 37 | License: GPL 2 38 | 39 | quaternion_matrix(), quaternion_from_matrix() in pymvg/ros_compat.py 40 | -------------------------------------------------------------------- 41 | 42 | Downloaded on 6 August, 2013 from 43 | http://www.lfd.uci.edu/~gohlke/code/transformations.py 44 | 45 | Author: Christoph Gohlke 46 | License: BSD 47 | 48 | estsimt() in pymvg/align.py 49 | --------------------------- 50 | 51 | Downloaded and converted on 27 October, 2013 from 52 | https://github.com/strawlab/MultiCamSelfCal/blob/master/MultiCamSelfCal/CoreFunctions/estsimt.m 53 | 54 | Author: Tomas Svoboda, svoboda@cmp.felk.cvut.cz 55 | License: unknown 56 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # pymvg - Python Multi-View Geometry 2 | 3 | PyMVG is a Python implementation of various computational camera 4 | geometry operations. 5 | 6 | ## Documentation 7 | 8 | Documentation is available [here](http://pymvg.readthedocs.org/). 9 | 10 | ## tests 11 | 12 | [![Build Status](https://travis-ci.org/strawlab/pymvg.png?branch=master)](https://travis-ci.org/strawlab/pymvg) 13 | 14 | [![Coverage Status](https://coveralls.io/repos/strawlab/pymvg/badge.png?branch=master)](https://coveralls.io/r/strawlab/pymvg?branch=master) 15 | 16 | PyMVG has a large collection of unit tests which ensure correctness 17 | and fulfilled expectations for use other software (see 'Ecosystem' in 18 | the documentation). To run the tests: 19 | 20 | pytest 21 | 22 | ## TODO 23 | 24 | - Implement extrinsic camera calibration to find camera pose when intrinsic parameters are known and image coordinates of known 3D points are given 25 | - Fix known failing test tests.external.mcsc.test_mcsc.test_mcsc_roundtrip 26 | - Implement OpenGL 3D -> 2d transform for augmented reality applications 27 | -------------------------------------------------------------------------------- /docs/Makefile: -------------------------------------------------------------------------------- 1 | # Makefile for Sphinx documentation 2 | # 3 | 4 | # You can set these variables from the command line. 5 | SPHINXOPTS = 6 | SPHINXBUILD = sphinx-build 7 | PAPER = 8 | BUILDDIR = build 9 | 10 | # Internal variables. 11 | PAPEROPT_a4 = -D latex_paper_size=a4 12 | PAPEROPT_letter = -D latex_paper_size=letter 13 | ALLSPHINXOPTS = -d $(BUILDDIR)/doctrees $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source 14 | # the i18n builder cannot share the environment and doctrees with the others 15 | I18NSPHINXOPTS = $(PAPEROPT_$(PAPER)) $(SPHINXOPTS) source 16 | 17 | .PHONY: help clean html dirhtml singlehtml pickle json htmlhelp qthelp devhelp epub latex latexpdf text man changes linkcheck doctest gettext 18 | 19 | help: 20 | @echo "Please use \`make ' where is one of" 21 | @echo " html to make standalone HTML files" 22 | @echo " dirhtml to make HTML files named index.html in directories" 23 | @echo " singlehtml to make a single large HTML file" 24 | @echo " pickle to make pickle files" 25 | @echo " json to make JSON files" 26 | @echo " htmlhelp to make HTML files and a HTML help project" 27 | @echo " qthelp to make HTML files and a qthelp project" 28 | @echo " devhelp to make HTML files and a Devhelp project" 29 | @echo " epub to make an epub" 30 | @echo " latex to make LaTeX files, you can set PAPER=a4 or PAPER=letter" 31 | @echo " latexpdf to make LaTeX files and run them through pdflatex" 32 | @echo " text to make text files" 33 | @echo " man to make manual pages" 34 | @echo " texinfo to make Texinfo files" 35 | @echo " info to make Texinfo files and run them through makeinfo" 36 | @echo " gettext to make PO message catalogs" 37 | @echo " changes to make an overview of all changed/added/deprecated items" 38 | @echo " linkcheck to check all external links for integrity" 39 | @echo " doctest to run all doctests embedded in the documentation (if enabled)" 40 | 41 | clean: 42 | -rm -rf $(BUILDDIR)/* 43 | 44 | source/images/camera_model.png: source/images/camera_model.svg 45 | inkscape -f $< --export-dpi=150 --export-background=white --export-png=$@ 46 | 47 | images: source/images/camera_model.png 48 | 49 | source/pymvg_camsystem_example.json: source/emit_sample_pymvg_file.py 50 | python $< $@ 51 | 52 | extra_sources: images source/pymvg_camsystem_example.json 53 | 54 | html: extra_sources 55 | $(SPHINXBUILD) -b html $(ALLSPHINXOPTS) $(BUILDDIR)/html 56 | @echo 57 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/html." 58 | 59 | dirhtml: extra_sources 60 | $(SPHINXBUILD) -b dirhtml $(ALLSPHINXOPTS) $(BUILDDIR)/dirhtml 61 | @echo 62 | @echo "Build finished. The HTML pages are in $(BUILDDIR)/dirhtml." 63 | 64 | singlehtml: extra_sources 65 | $(SPHINXBUILD) -b singlehtml $(ALLSPHINXOPTS) $(BUILDDIR)/singlehtml 66 | @echo 67 | @echo "Build finished. The HTML page is in $(BUILDDIR)/singlehtml." 68 | 69 | pickle: extra_sources 70 | $(SPHINXBUILD) -b pickle $(ALLSPHINXOPTS) $(BUILDDIR)/pickle 71 | @echo 72 | @echo "Build finished; now you can process the pickle files." 73 | 74 | json: extra_sources 75 | $(SPHINXBUILD) -b json $(ALLSPHINXOPTS) $(BUILDDIR)/json 76 | @echo 77 | @echo "Build finished; now you can process the JSON files." 78 | 79 | htmlhelp: 80 | $(SPHINXBUILD) -b htmlhelp $(ALLSPHINXOPTS) $(BUILDDIR)/htmlhelp 81 | @echo 82 | @echo "Build finished; now you can run HTML Help Workshop with the" \ 83 | ".hhp project file in $(BUILDDIR)/htmlhelp." 84 | 85 | qthelp: 86 | $(SPHINXBUILD) -b qthelp $(ALLSPHINXOPTS) $(BUILDDIR)/qthelp 87 | @echo 88 | @echo "Build finished; now you can run "qcollectiongenerator" with the" \ 89 | ".qhcp project file in $(BUILDDIR)/qthelp, like this:" 90 | @echo "# qcollectiongenerator $(BUILDDIR)/qthelp/PyMVG.qhcp" 91 | @echo "To view the help file:" 92 | @echo "# assistant -collectionFile $(BUILDDIR)/qthelp/PyMVG.qhc" 93 | 94 | devhelp: 95 | $(SPHINXBUILD) -b devhelp $(ALLSPHINXOPTS) $(BUILDDIR)/devhelp 96 | @echo 97 | @echo "Build finished." 98 | @echo "To view the help file:" 99 | @echo "# mkdir -p $$HOME/.local/share/devhelp/PyMVG" 100 | @echo "# ln -s $(BUILDDIR)/devhelp $$HOME/.local/share/devhelp/PyMVG" 101 | @echo "# devhelp" 102 | 103 | epub: 104 | $(SPHINXBUILD) -b epub $(ALLSPHINXOPTS) $(BUILDDIR)/epub 105 | @echo 106 | @echo "Build finished. The epub file is in $(BUILDDIR)/epub." 107 | 108 | latex: extra_sources 109 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 110 | @echo 111 | @echo "Build finished; the LaTeX files are in $(BUILDDIR)/latex." 112 | @echo "Run \`make' in that directory to run these through (pdf)latex" \ 113 | "(use \`make latexpdf' here to do that automatically)." 114 | 115 | latexpdf: extra_sources 116 | $(SPHINXBUILD) -b latex $(ALLSPHINXOPTS) $(BUILDDIR)/latex 117 | @echo "Running LaTeX files through pdflatex..." 118 | $(MAKE) -C $(BUILDDIR)/latex all-pdf 119 | @echo "pdflatex finished; the PDF files are in $(BUILDDIR)/latex." 120 | 121 | text: 122 | $(SPHINXBUILD) -b text $(ALLSPHINXOPTS) $(BUILDDIR)/text 123 | @echo 124 | @echo "Build finished. The text files are in $(BUILDDIR)/text." 125 | 126 | man: 127 | $(SPHINXBUILD) -b man $(ALLSPHINXOPTS) $(BUILDDIR)/man 128 | @echo 129 | @echo "Build finished. The manual pages are in $(BUILDDIR)/man." 130 | 131 | texinfo: 132 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 133 | @echo 134 | @echo "Build finished. The Texinfo files are in $(BUILDDIR)/texinfo." 135 | @echo "Run \`make' in that directory to run these through makeinfo" \ 136 | "(use \`make info' here to do that automatically)." 137 | 138 | info: 139 | $(SPHINXBUILD) -b texinfo $(ALLSPHINXOPTS) $(BUILDDIR)/texinfo 140 | @echo "Running Texinfo files through makeinfo..." 141 | make -C $(BUILDDIR)/texinfo info 142 | @echo "makeinfo finished; the Info files are in $(BUILDDIR)/texinfo." 143 | 144 | gettext: 145 | $(SPHINXBUILD) -b gettext $(I18NSPHINXOPTS) $(BUILDDIR)/locale 146 | @echo 147 | @echo "Build finished. The message catalogs are in $(BUILDDIR)/locale." 148 | 149 | changes: 150 | $(SPHINXBUILD) -b changes $(ALLSPHINXOPTS) $(BUILDDIR)/changes 151 | @echo 152 | @echo "The overview file is in $(BUILDDIR)/changes." 153 | 154 | linkcheck: 155 | $(SPHINXBUILD) -b linkcheck $(ALLSPHINXOPTS) $(BUILDDIR)/linkcheck 156 | @echo 157 | @echo "Link check complete; look for any errors in the above output " \ 158 | "or in $(BUILDDIR)/linkcheck/output.txt." 159 | 160 | doctest: 161 | $(SPHINXBUILD) -b doctest $(ALLSPHINXOPTS) $(BUILDDIR)/doctest 162 | @echo "Testing of doctests in the sources finished, look at the " \ 163 | "results in $(BUILDDIR)/doctest/output.txt." 164 | -------------------------------------------------------------------------------- /docs/source/api_reference.rst: -------------------------------------------------------------------------------- 1 | API Reference 2 | ============= 3 | 4 | .. autoclass:: pymvg.camera_model.CameraModel 5 | :members: 6 | 7 | .. autoclass:: pymvg.multi_camera_system.MultiCameraSystem 8 | :members: 9 | -------------------------------------------------------------------------------- /docs/source/camera_model.rst: -------------------------------------------------------------------------------- 1 | Camera Model 2 | ============ 3 | 4 | single camera model 5 | ------------------- 6 | 7 | The core of PyMVG is a camera model that is compatible with the 8 | calibration outputs of `OpenCV `_ and 9 | `MultiCamSelfCal `_. 10 | 11 | .. image:: images/camera_model.png 12 | 13 | In the above image, you can see that this camera model consists of a 14 | linear pinhole projection model followed by a nonlinear distortion 15 | model. The pinhole model is specified completely by the 3x4 matrix M 16 | (or, equivalently, the 3x3 intrinsic matrix P, the 3x3 ortho-normal 17 | rotation matrix Q, and the translation vector t). The nonlinear 18 | distortion model is specified completely by elements of the intrinsic 19 | matrix of the pinhole model and several distortion terms. 20 | 21 | camera system (multiple cameras) 22 | -------------------------------- 23 | 24 | PyMVG represents a camera system with the MultiCameraSystem class. You 25 | create an instance with a list of individual camera instances. The 26 | class provides additional methods for triangulation of 3D points and 27 | so on. 28 | -------------------------------------------------------------------------------- /docs/source/conf.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | # 3 | # PyMVG documentation build configuration file, created by 4 | # sphinx-quickstart on Sat Nov 2 17:43:20 2013. 5 | # 6 | # This file is execfile()d with the current directory set to its containing dir. 7 | # 8 | # Note that not all possible configuration values are present in this 9 | # autogenerated file. 10 | # 11 | # All configuration values have a default; values that are commented out 12 | # serve to show the default. 13 | 14 | import sys, os 15 | 16 | # If extensions (or modules to document with autodoc) are in another directory, 17 | # add these directories to sys.path here. If the directory is relative to the 18 | # documentation root, use os.path.abspath to make it absolute, like shown here. 19 | #sys.path.insert(0, os.path.abspath('.')) 20 | 21 | # -- General configuration ----------------------------------------------------- 22 | 23 | # If your documentation needs a minimal Sphinx version, state it here. 24 | #needs_sphinx = '1.0' 25 | 26 | # Add any Sphinx extension module names here, as strings. They can be extensions 27 | # coming with Sphinx (named 'sphinx.ext.*') or your custom ones. 28 | extensions = ['sphinx.ext.autodoc', 'sphinx.ext.doctest', 'sphinx.ext.todo', 'sphinx.ext.coverage', 'sphinx.ext.mathjax', 'sphinx.ext.viewcode'] 29 | 30 | extensions.extend([ 'matplotlib.sphinxext.plot_directive' ]) 31 | 32 | # Add any paths that contain templates here, relative to this directory. 33 | templates_path = ['_templates'] 34 | 35 | # The suffix of source filenames. 36 | source_suffix = '.rst' 37 | 38 | # The encoding of source files. 39 | #source_encoding = 'utf-8-sig' 40 | 41 | # The master toctree document. 42 | master_doc = 'index' 43 | 44 | # General information about the project. 45 | project = u'PyMVG' 46 | copyright = u'2013, Andrew Straw' 47 | 48 | # The version info for the project you're documenting, acts as replacement for 49 | # |version| and |release|, also used in various other places throughout the 50 | # built documents. 51 | # 52 | # The short X.Y version. 53 | version = '1.0' 54 | # The full version, including alpha/beta/rc tags. 55 | release = '1.0' 56 | 57 | # The language for content autogenerated by Sphinx. Refer to documentation 58 | # for a list of supported languages. 59 | #language = None 60 | 61 | # There are two options for replacing |today|: either, you set today to some 62 | # non-false value, then it is used: 63 | #today = '' 64 | # Else, today_fmt is used as the format for a strftime call. 65 | #today_fmt = '%B %d, %Y' 66 | 67 | # List of patterns, relative to source directory, that match files and 68 | # directories to ignore when looking for source files. 69 | exclude_patterns = [] 70 | 71 | # The reST default role (used for this markup: `text`) to use for all documents. 72 | #default_role = None 73 | 74 | # If true, '()' will be appended to :func: etc. cross-reference text. 75 | #add_function_parentheses = True 76 | 77 | # If true, the current module name will be prepended to all description 78 | # unit titles (such as .. function::). 79 | #add_module_names = True 80 | 81 | # If true, sectionauthor and moduleauthor directives will be shown in the 82 | # output. They are ignored by default. 83 | #show_authors = False 84 | 85 | # The name of the Pygments (syntax highlighting) style to use. 86 | pygments_style = 'sphinx' 87 | 88 | # A list of ignored prefixes for module index sorting. 89 | #modindex_common_prefix = [] 90 | 91 | 92 | # -- Options for HTML output --------------------------------------------------- 93 | 94 | # The theme to use for HTML and HTML Help pages. See the documentation for 95 | # a list of builtin themes. 96 | html_theme = 'default' 97 | 98 | # Theme options are theme-specific and customize the look and feel of a theme 99 | # further. For a list of options available for each theme, see the 100 | # documentation. 101 | #html_theme_options = {} 102 | 103 | # Add any paths that contain custom themes here, relative to this directory. 104 | #html_theme_path = [] 105 | 106 | # The name for this set of Sphinx documents. If None, it defaults to 107 | # " v documentation". 108 | #html_title = None 109 | 110 | # A shorter title for the navigation bar. Default is the same as html_title. 111 | #html_short_title = None 112 | 113 | # The name of an image file (relative to this directory) to place at the top 114 | # of the sidebar. 115 | #html_logo = None 116 | 117 | # The name of an image file (within the static path) to use as favicon of the 118 | # docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 119 | # pixels large. 120 | #html_favicon = None 121 | 122 | # Add any paths that contain custom static files (such as style sheets) here, 123 | # relative to this directory. They are copied after the builtin static files, 124 | # so a file named "default.css" will overwrite the builtin "default.css". 125 | html_static_path = ['_static'] 126 | 127 | # If not '', a 'Last updated on:' timestamp is inserted at every page bottom, 128 | # using the given strftime format. 129 | #html_last_updated_fmt = '%b %d, %Y' 130 | 131 | # If true, SmartyPants will be used to convert quotes and dashes to 132 | # typographically correct entities. 133 | #html_use_smartypants = True 134 | 135 | # Custom sidebar templates, maps document names to template names. 136 | #html_sidebars = {} 137 | 138 | # Additional templates that should be rendered to pages, maps page names to 139 | # template names. 140 | #html_additional_pages = {} 141 | 142 | # If false, no module index is generated. 143 | #html_domain_indices = True 144 | 145 | # If false, no index is generated. 146 | #html_use_index = True 147 | 148 | # If true, the index is split into individual pages for each letter. 149 | #html_split_index = False 150 | 151 | # If true, links to the reST sources are added to the pages. 152 | #html_show_sourcelink = True 153 | 154 | # If true, "Created using Sphinx" is shown in the HTML footer. Default is True. 155 | #html_show_sphinx = True 156 | 157 | # If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. 158 | #html_show_copyright = True 159 | 160 | # If true, an OpenSearch description file will be output, and all pages will 161 | # contain a tag referring to it. The value of this option must be the 162 | # base URL from which the finished HTML is served. 163 | #html_use_opensearch = '' 164 | 165 | # This is the file name suffix for HTML files (e.g. ".xhtml"). 166 | #html_file_suffix = None 167 | 168 | # Output file base name for HTML help builder. 169 | htmlhelp_basename = 'PyMVGdoc' 170 | 171 | 172 | # -- Options for LaTeX output -------------------------------------------------- 173 | 174 | latex_elements = { 175 | # The paper size ('letterpaper' or 'a4paper'). 176 | #'papersize': 'letterpaper', 177 | 178 | # The font size ('10pt', '11pt' or '12pt'). 179 | #'pointsize': '10pt', 180 | 181 | # Additional stuff for the LaTeX preamble. 182 | #'preamble': '', 183 | } 184 | 185 | # Grouping the document tree into LaTeX files. List of tuples 186 | # (source start file, target name, title, author, documentclass [howto/manual]). 187 | latex_documents = [ 188 | ('index', 'PyMVG.tex', u'PyMVG Documentation', 189 | u'Andrew Straw', 'manual'), 190 | ] 191 | 192 | # The name of an image file (relative to this directory) to place at the top of 193 | # the title page. 194 | #latex_logo = None 195 | 196 | # For "manual" documents, if this is true, then toplevel headings are parts, 197 | # not chapters. 198 | #latex_use_parts = False 199 | 200 | # If true, show page references after internal links. 201 | #latex_show_pagerefs = False 202 | 203 | # If true, show URL addresses after external links. 204 | #latex_show_urls = False 205 | 206 | # Documents to append as an appendix to all manuals. 207 | #latex_appendices = [] 208 | 209 | # If false, no module index is generated. 210 | #latex_domain_indices = True 211 | 212 | 213 | # -- Options for manual page output -------------------------------------------- 214 | 215 | # One entry per manual page. List of tuples 216 | # (source start file, name, description, authors, manual section). 217 | man_pages = [ 218 | ('index', 'pymvg', u'PyMVG Documentation', 219 | [u'Andrew Straw'], 1) 220 | ] 221 | 222 | # If true, show URL addresses after external links. 223 | #man_show_urls = False 224 | 225 | 226 | # -- Options for Texinfo output ------------------------------------------------ 227 | 228 | # Grouping the document tree into Texinfo files. List of tuples 229 | # (source start file, target name, title, author, 230 | # dir menu entry, description, category) 231 | texinfo_documents = [ 232 | ('index', 'PyMVG', u'PyMVG Documentation', 233 | u'Andrew Straw', 'PyMVG', 'One line description of project.', 234 | 'Miscellaneous'), 235 | ] 236 | 237 | # Documents to append as an appendix to all manuals. 238 | #texinfo_appendices = [] 239 | 240 | # If false, no module index is generated. 241 | #texinfo_domain_indices = True 242 | 243 | # How to display URL addresses: 'footnote', 'no', or 'inline'. 244 | #texinfo_show_urls = 'footnote' 245 | -------------------------------------------------------------------------------- /docs/source/emit_sample_pymvg_file.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | from pymvg.camera_model import CameraModel 3 | from pymvg.multi_camera_system import MultiCameraSystem 4 | import sys 5 | 6 | def make_default_system(): 7 | '''helper function to generate an instance of MultiCameraSystem''' 8 | lookat = np.array( (0.0, 0.0, 0.0)) 9 | 10 | center1 = np.array( (0.0, 0.0, 0.9) ) 11 | distortion1 = np.array( [0.2, 0.3, 0.1, 0.1, 0.1] ) 12 | cam1 = CameraModel.load_camera_simple(name='cam1', 13 | fov_x_degrees=90, 14 | eye=center1, 15 | lookat=lookat, 16 | distortion_coefficients=distortion1, 17 | ) 18 | 19 | center2 = np.array( (0.5, -0.8, 0.0) ) 20 | cam2 = CameraModel.load_camera_simple(name='cam2', 21 | fov_x_degrees=90, 22 | eye=center2, 23 | lookat=lookat, 24 | ) 25 | 26 | center3 = np.array( (0.5, 0.5, 0.0) ) 27 | cam3 = CameraModel.load_camera_simple(name='cam3', 28 | fov_x_degrees=90, 29 | eye=center3, 30 | lookat=lookat, 31 | ) 32 | 33 | system = MultiCameraSystem([cam1,cam2,cam3]) 34 | return system 35 | 36 | def main(): 37 | out_fname = sys.argv[1] 38 | system1 = make_default_system() 39 | system1.save_to_pymvg_file( out_fname ) 40 | 41 | # just make sure we can actually read it! 42 | system2 = MultiCameraSystem.from_pymvg_file( out_fname ) 43 | assert system1==system2 44 | 45 | if __name__=='__main__': 46 | main() 47 | -------------------------------------------------------------------------------- /docs/source/images/camera_model.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/pymvg/1e7ad0bdd5242146b3c916ea6dcbce101261928b/docs/source/images/camera_model.png -------------------------------------------------------------------------------- /docs/source/index.rst: -------------------------------------------------------------------------------- 1 | PyMVG documentation 2 | =================== 3 | 4 | `PyMVG `_ is a Python implementation of 5 | various computational camera geometry operations. 6 | 7 | Features: 8 | 9 | - triangulate 2D features from multiple calibrated cameras into a 10 | single 3D point (using algorithm from `the classic textbook by 11 | Hartley & Zisserman 12 | `_). 13 | `[example] `_ 14 | - load/save camera calibrations from `ROS `_ (which 15 | uses `OpenCV `_) 16 | - load/save camera system calibrations from `MultiCamSelfCal 17 | `_ 18 | - complete implementation of OpenCV camera model in pure Python in a 19 | `single file 20 | `_ 21 | for easy understanding 22 | - complete implementation of DLT camera calibration procedure 23 | - completely vectorized code for rapid operation on many points using 24 | `numpy `_ 25 | - completely written in `Python `_ 26 | - plotting utilities `[example 1] 27 | `_ 28 | `[example 2] 29 | `_ 30 | 31 | It contains a complete re-implementation of the OpenCV camera model 32 | and can thus use calibrations made by or for OpenCV. PyMVG is entirely 33 | written in Python, and thus -- depending on your preferences -- it may 34 | be significantly easier to understand than the equivalent OpenCV 35 | implementation. PyMVG makes extensive use of `numpy 36 | `_, and thus when called on large batches of points, 37 | is no slower than native code. 38 | 39 | Ecosystem 40 | --------- 41 | 42 | PyMVG is designed to interoperate with `OpenCV `_, 43 | `ROS `_, and `MultiCamSelfCal 44 | `_. Unit tests ensure 45 | exact compatibility with the relevant parts of these packages. 46 | 47 | See also `opengl-hz `_. 48 | 49 | Development 50 | ----------- 51 | 52 | All development is done on `our github repository 53 | `_. 54 | 55 | .. toctree:: 56 | 57 | pymvg_file_format 58 | plotting_utilities 59 | camera_model 60 | api_reference 61 | 62 | Indices and tables 63 | ================== 64 | 65 | * :ref:`genindex` 66 | * :ref:`modindex` 67 | * :ref:`search` 68 | 69 | -------------------------------------------------------------------------------- /docs/source/plotting_utilities.rst: -------------------------------------------------------------------------------- 1 | Plotting utilities 2 | ================== 3 | 4 | Given the above example, we can plot the camera system. 5 | 6 | .. plot:: pyplots/plot_camsystem_example.py 7 | :include-source: 8 | -------------------------------------------------------------------------------- /docs/source/pymvg_camsystem_example.json: -------------------------------------------------------------------------------- 1 | { "__pymvg_file_version__": "1.0", 2 | "camera_system": [ 3 | {"name": "cam1", 4 | "width": 640, 5 | "height": 480, 6 | "P": [[ 320.0, 0, 319.99999999999994, 0 ], 7 | [ 0, 320.00000000000006, 240.0, 0 ], 8 | [ 0, 0, 1.0, 0 ]], 9 | "K": [[ 320.0, 0, 319.99999999999994 ], 10 | [ 0, 320.00000000000006, 240.0 ], 11 | [ 0, 0, 1.0 ]], 12 | "D": [ 0.2, 0.3, 0.1, 0.1, 0.1 ], 13 | "R": [[ 1.0, 0, 0 ], 14 | [ 0, 1.0, 0 ], 15 | [ 0, 0, 1.0 ]], 16 | "Q": [[ -1.0000000000000004, 0, 0 ], 17 | [ 0, 1.0, 0 ], 18 | [ 0, 0, -1.0000000000000004 ]], 19 | "translation": [ 0, 0, 0.9000000000000005 ] 20 | }, 21 | {"name": "cam2", 22 | "width": 640, 23 | "height": 480, 24 | "P": [[ 320.0, 0, 319.99999999999994, 0 ], 25 | [ 0, 320.00000000000006, 240.0, 0 ], 26 | [ 0, 0, 1.0, 0 ]], 27 | "K": [[ 320.0, 0, 319.99999999999994 ], 28 | [ 0, 320.00000000000006, 240.0 ], 29 | [ 0, 0, 1.0 ]], 30 | "D": [ 0, 0, 0, 0, 0 ], 31 | "R": [[ 1.0, 0, 0 ], 32 | [ 0, 1.0, 0 ], 33 | [ 0, 0, 1.0 ]], 34 | "Q": [[ 0, 0, 0.9999999999999999 ], 35 | [ 0.847998304005088, 0.5299989400031799, 0 ], 36 | [ -0.5299989400031798, 0.847998304005088, 0 ]], 37 | "translation": [ 0, 0, 0.9433981132056602 ] 38 | }, 39 | {"name": "cam3", 40 | "width": 640, 41 | "height": 480, 42 | "P": [[ 320.0, 0, 319.99999999999994, 0 ], 43 | [ 0, 320.00000000000006, 240.0, 0 ], 44 | [ 0, 0, 1.0, 0 ]], 45 | "K": [[ 320.0, 0, 319.99999999999994 ], 46 | [ 0, 320.00000000000006, 240.0 ], 47 | [ 0, 0, 1.0 ]], 48 | "D": [ 0, 0, 0, 0, 0 ], 49 | "R": [[ 1.0, 0, 0 ], 50 | [ 0, 1.0, 0 ], 51 | [ 0, 0, 1.0 ]], 52 | "Q": [[ 0, 0, 1.0000000000000002 ], 53 | [ -0.7071067811865475, 0.7071067811865477, 0 ], 54 | [ -0.7071067811865478, -0.7071067811865475, 0 ]], 55 | "translation": [ 0, 0, 0.7071067811865475 ] 56 | } 57 | ] 58 | } -------------------------------------------------------------------------------- /docs/source/pymvg_file_format.rst: -------------------------------------------------------------------------------- 1 | PyMVG file format 2 | ================= 3 | 4 | The PyMVG file format specifies a camera system completely. The file 5 | is valid JSON. Here is an example that specifies a system of 3 6 | cameras: 7 | 8 | .. literalinclude:: pymvg_camsystem_example.json 9 | -------------------------------------------------------------------------------- /docs/source/pyplots/plot_camsystem_example.py: -------------------------------------------------------------------------------- 1 | from pymvg import CameraModel, MultiCameraSystem 2 | from pymvg.plot_utils import plot_system 3 | 4 | import os 5 | 6 | import matplotlib.pyplot as plt 7 | from mpl_toolkits.mplot3d import Axes3D 8 | 9 | fname = os.path.join('..','pymvg_camsystem_example.json') 10 | system = MultiCameraSystem.from_pymvg_file( fname ) 11 | 12 | fig = plt.figure() 13 | ax = fig.add_subplot(1,1,1, projection='3d') 14 | plot_system( ax, system ) 15 | ax.set_xlabel('x'); ax.set_ylabel('y'); ax.set_zlabel('z') 16 | ax.set_xlim(-0.8,0.8); ax.set_ylim(-0.8,0.8); ax.set_zlim(-0.8,0.8) 17 | plt.show() 18 | 19 | -------------------------------------------------------------------------------- /examples/plot_camera_system.py: -------------------------------------------------------------------------------- 1 | from pymvg.multi_camera_system import build_example_system 2 | from pymvg.plot_utils import plot_system 3 | import matplotlib.pyplot as plt 4 | from mpl_toolkits.mplot3d import Axes3D 5 | import numpy as np 6 | 7 | n=6 8 | z=5.0 9 | system = build_example_system(n=n,z=z) 10 | 11 | fig = plt.figure() 12 | ax = fig.add_subplot(111, projection='3d') 13 | 14 | plot_system( ax, system, scale=z/5.0 ) 15 | 16 | if 1: 17 | # put some points to force mpl's view dimensions 18 | pts = np.array([[0,0,0], 19 | [2*n, 2*z, 2*z]]) 20 | ax.plot( pts[:,0], pts[:,1], pts[:,2], 'k.') 21 | 22 | ax.set_xlabel('x') 23 | ax.set_ylabel('y') 24 | ax.set_zlabel('z') 25 | plt.show() 26 | -------------------------------------------------------------------------------- /examples/plot_cameras.py: -------------------------------------------------------------------------------- 1 | from pymvg.plot_utils import plot_camera 2 | from pymvg.multi_camera_system import build_example_system 3 | import matplotlib.pyplot as plt 4 | from mpl_toolkits.mplot3d import Axes3D 5 | import numpy as np 6 | 7 | n=6 8 | z=5.0 9 | system = build_example_system(n=n,z=z) 10 | 11 | fig = plt.figure() 12 | ax = fig.add_subplot(111, projection='3d') 13 | 14 | for name in system.get_names(): 15 | plot_camera( ax, system.get_camera(name), scale = z/5.0 ) 16 | 17 | if 1: 18 | # put some points to force mpl's view dimensions 19 | pts = np.array([[0,0,0], 20 | [2*n, 2*z, 2*z]]) 21 | ax.plot( pts[:,0], pts[:,1], pts[:,2], 'k.') 22 | 23 | ax.set_xlabel('x') 24 | ax.set_ylabel('y') 25 | ax.set_zlabel('z') 26 | plt.show() 27 | -------------------------------------------------------------------------------- /examples/ros/draw_rviz_frustum.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import argparse 4 | import json 5 | import threading 6 | 7 | import roslib; roslib.load_manifest('flyvr') 8 | roslib.load_manifest('visualization_msgs') 9 | import rospy 10 | 11 | import tf.transformations 12 | from sensor_msgs.msg import CameraInfo 13 | import sensor_msgs.msg 14 | import std_msgs.msg 15 | from geometry_msgs.msg import Point, Pose, Transform 16 | import geometry_msgs.msg 17 | import tf.broadcaster 18 | import tf.msg 19 | from visualization_msgs.msg import Marker, MarkerArray 20 | 21 | import flyvr.simple_geom as simple_geom 22 | import flyvr.display_client as display_client 23 | from pymvg.camera_model import CameraModel 24 | import pymvg.extern.ros.rviz_utils as rviz_utils 25 | 26 | class MyApp: 27 | def __init__(self,name,scale=1.0): 28 | self.name = name 29 | self.scale = scale 30 | self.intrinsics = None 31 | self.translation = None 32 | self.rotation = None 33 | self._lock = threading.Lock() 34 | self.tl = tf.TransformListener() 35 | self.cam = None 36 | 37 | ci_name = self.get_frame_id()+'/camera_info' 38 | rospy.loginfo('now listening for CameraInfo message on topic %r'%ci_name) 39 | 40 | rospy.Subscriber(ci_name, 41 | CameraInfo, self.on_camera_info) 42 | 43 | self.topic_name = self.get_frame_id()+'/frustum' 44 | rospy.loginfo('publishing frustum (scale %s) at %r'%(self.scale, 45 | self.topic_name)) 46 | self.publisher = rospy.Publisher(self.topic_name, MarkerArray) 47 | 48 | rospy.loginfo('now listening for transform at %r'%self.get_frame_id()) 49 | rospy.Timer(rospy.Duration(1.0/20.0), self.on_timer) # 20 fps 50 | 51 | def get_frame_id(self): 52 | return '/'+self.name 53 | 54 | def on_camera_info(self, msg): 55 | with self._lock: 56 | self.intrinsics = msg 57 | 58 | def on_timer(self, _): 59 | now = rospy.Time.now() 60 | try: 61 | translation,rotation = self.tl.lookupTransform('/map', 62 | self.get_frame_id(), 63 | now) 64 | except (tf.LookupException, tf.ExtrapolationException) as err: 65 | return 66 | 67 | with self._lock: 68 | self.translation = translation 69 | self.rotation = rotation 70 | 71 | self.new_data() 72 | 73 | def new_data(self): 74 | with self._lock: 75 | if (self.translation is None or 76 | self.rotation is None or 77 | self.intrinsics is None): 78 | return 79 | newcam = CameraModel.load_camera_from_ROS_tf( translation=self.translation, 80 | rotation=self.rotation, 81 | intrinsics=self.intrinsics, 82 | name=self.get_frame_id(), 83 | ) 84 | self.cam = newcam 85 | 86 | self.draw() 87 | 88 | def draw(self): 89 | r = rviz_utils.get_frustum_markers( self.cam, scale=self.scale ) 90 | self.publisher.publish(r['markers']) 91 | 92 | def run(self): 93 | rospy.spin() 94 | 95 | if __name__ == '__main__': 96 | rospy.init_node('draw_rviz_frustum',anonymous=True) 97 | 98 | parser = argparse.ArgumentParser() 99 | 100 | parser.add_argument('--name', type=str, default='cam_0') 101 | parser.add_argument('--scale', type=float, default=1.0) 102 | 103 | argv = rospy.myargv() 104 | args = parser.parse_args(argv[1:]) 105 | 106 | app = MyApp(args.name,scale=args.scale) 107 | app.run() 108 | -------------------------------------------------------------------------------- /examples/ros/publish_sample_cameras.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from pymvg.multi_camera_system import build_example_system 4 | from pymvg.extern.ros.ros_publisher import ROSPublisher 5 | 6 | import roslib # ROS is required for this example. See http://ros.org 7 | roslib.load_manifest('rospy') 8 | import rospy 9 | 10 | cam_pubs = [] 11 | 12 | rospy.init_node("publish_sample_cameras") 13 | 14 | n=6 15 | z=5.0 16 | system = build_example_system(n=n,z=z) 17 | 18 | for name in system.get_names(): 19 | cam_pubs.append(ROSPublisher(system.get_camera(name))) 20 | 21 | rospy.spin() 22 | -------------------------------------------------------------------------------- /examples/ros/ros_rviz_config.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorR=0 2 | Background\ ColorG=0 3 | Background\ ColorB=0 4 | Fixed\ Frame=/map 5 | Target\ Frame=/map 6 | Grid.Alpha=0.5 7 | Grid.Cell\ Size=1 8 | Grid.ColorR=0.5 9 | Grid.ColorG=0.5 10 | Grid.ColorB=0.5 11 | Grid.Enabled=1 12 | Grid.Line\ Style=0 13 | Grid.Line\ Width=0.03 14 | Grid.Normal\ Cell\ Count=0 15 | Grid.OffsetX=0 16 | Grid.OffsetY=0 17 | Grid.OffsetZ=0 18 | Grid.Plane=0 19 | Grid.Plane\ Cell\ Count=10 20 | Grid.Reference\ Frame= 21 | TF.All\ Enabled=1 22 | TF.Enabled=1 23 | TF.Frame\ Timeout=15 24 | TF.Marker\ Scale=0.1 25 | TF.Show\ Arrows=1 26 | TF.Show\ Axes=1 27 | TF.Show\ Names=1 28 | TF.Update\ Interval=0.05 29 | cam0\ frustum.=1 30 | cam0\ frustum.Enabled=1 31 | cam0\ frustum.Marker\ Array\ Topic=/cam_0/frustum 32 | cam1\ frustum.=1 33 | cam1\ frustum.Enabled=1 34 | cam1\ frustum.Marker\ Array\ Topic=/cam_1/frustum 35 | cam2\ frustum.=1 36 | cam2\ frustum.Enabled=1 37 | cam2\ frustum.Marker\ Array\ Topic=/cam_2/frustum 38 | Tool\ 2D\ Nav\ GoalTopic=goal 39 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 40 | Camera\ Type=rviz::OrbitViewController 41 | Camera\ Config=0.94779 4.83079 19.9702 0 -1.90735e-06 1.90735e-06 42 | Property\ Grid\ State=selection=cam0 frustum.Enabled;expanded=.Global Options,TF./cam_0/cam_0,TF./cam_1/cam_1,TF./cam_2/cam_2,TF./cam_3/cam_3,TF./cam_4/cam_4,TF./cam_5/cam_5,TF./map/map,TF.Enabled.TF.Tree,TF./mapTree/map,TF./cam_0Tree/cam_0,TF./cam_1Tree/cam_1,TF./cam_2Tree/cam_2,TF./cam_3Tree/cam_3,TF./cam_4Tree/cam_4,TF./cam_5Tree/cam_5,cam0 frustum.Enabled.cam0 frustum.Status,cam0 frustum.Enabled.cam0 frustum.Namespaces,cam1 frustum.Enabled.cam1 frustum.Namespaces,cam2 frustum.Enabled.cam2 frustum.Namespaces;scrollpos=0,0;splitterpos=150,301;ispageselected=1 43 | [Display0] 44 | Name=TF 45 | Package=rviz 46 | ClassName=rviz::TFDisplay 47 | [Display1] 48 | Name=Grid 49 | Package=rviz 50 | ClassName=rviz::GridDisplay 51 | [Display2] 52 | Name=cam0 frustum 53 | Package=rviz 54 | ClassName=rviz::MarkerArrayDisplay 55 | [Display3] 56 | Name=cam1 frustum 57 | Package=rviz 58 | ClassName=rviz::MarkerArrayDisplay 59 | [Display4] 60 | Name=cam2 frustum 61 | Package=rviz 62 | ClassName=rviz::MarkerArrayDisplay 63 | [TF.] 64 | cam_0Enabled=1 65 | cam_1Enabled=1 66 | cam_2Enabled=1 67 | cam_3Enabled=1 68 | cam_4Enabled=1 69 | cam_5Enabled=1 70 | mapEnabled=1 71 | -------------------------------------------------------------------------------- /examples/triangulate_point.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | from pymvg.multi_camera_system import build_example_system 3 | import numpy as np 4 | 5 | system = build_example_system() 6 | point_3d_expected = np.array([1.0, 2.0, 3.0]) 7 | print('Original: %r'%(point_3d_expected,)) 8 | 9 | # find perfect 2D projection of this original 3D point 10 | data = [] 11 | for camera_name in system.get_names(): 12 | this_pt2d = system.find2d(camera_name,point_3d_expected) 13 | data.append( (camera_name,this_pt2d) ) 14 | print('%r: %r'%(camera_name,this_pt2d)) 15 | 16 | # now triangulate 3D point 17 | point_3d_actual = system.find3d(data) 18 | print('Result ----> %r'%(point_3d_actual,)) 19 | 20 | -------------------------------------------------------------------------------- /pymvg/__init__.py: -------------------------------------------------------------------------------- 1 | __version__ = '2.1.0' 2 | -------------------------------------------------------------------------------- /pymvg/align.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def estsimt(X1,X2): 4 | # from estsimt.m in MultiCameSelfCal 5 | 6 | # ESTimate SIMilarity Transformation 7 | # 8 | # [s,R,T] = estsimt(X1,X2) 9 | # 10 | # X1,X2 ... 3xN matrices with corresponding 3D points 11 | # 12 | # X2 = s*R*X1 + T 13 | # s ... scalar scale 14 | # R ... 3x3 rotation matrix 15 | # T ... 3x1 translation vector 16 | # 17 | # This is done according to the paper: 18 | # "Least-Squares Fitting of Two 3-D Point Sets" 19 | # by K.S. Arun, T. S. Huang and S. D. Blostein 20 | 21 | N = X1.shape[1] 22 | if N != X2.shape[1]: 23 | raise ValueError('both X1 and X2 must have same number of points') 24 | 25 | X1cent = np.mean(X1,axis=1) 26 | X2cent = np.mean(X2,axis=1) 27 | # normalize coordinate systems for both set of points 28 | x1 = X1 - X1cent[:,np.newaxis] 29 | x2 = X2 - X2cent[:,np.newaxis] 30 | 31 | # mutual distances 32 | d1 = x1[:,1:]-x1[:,:-1] 33 | d2 = x2[:,1:]-x2[:,:-1] 34 | ds1 = np.sqrt( np.sum( d1**2, axis=0) ) 35 | ds2 = np.sqrt( np.sum( d2**2, axis=0) ) 36 | 37 | scales = ds2/ds1 38 | s = np.median( scales ) 39 | 40 | # undo scale 41 | x1s = s*x1 42 | 43 | # finding rotation 44 | H = np.zeros((3,3)) 45 | for i in range(N): 46 | tmp1 = x1s[:,i,np.newaxis] 47 | tmp2 = x2[np.newaxis,:,i] 48 | tmp = np.dot(tmp1,tmp2) 49 | H += tmp 50 | 51 | U,S,Vt = np.linalg.svd(H) 52 | V = Vt.T 53 | X = np.dot(V,U.T) 54 | R=X 55 | 56 | T = X2cent - s*np.dot(R,X1cent) 57 | return s,R,T 58 | 59 | def build_xform(s,R,t): 60 | T = np.zeros((4,4),dtype=float) 61 | T[:3,:3] = R 62 | T = s*T 63 | T[:3,3] = t 64 | T[3,3]=1.0 65 | return T 66 | 67 | def align_points( s,R,T, X ): 68 | T = build_xform(s,R,T) 69 | if X.shape[0]==3: 70 | # make homogeneous 71 | Xnew = np.ndarray((4,X.shape[1]),dtype=X.dtype) 72 | Xnew[3,:].fill(1) 73 | Xnew[:3,:] = X 74 | X = Xnew 75 | X = np.dot(T,X) 76 | return X 77 | 78 | def align_M( s,R,T, P ): 79 | T = build_xform(s,R,T) 80 | P = np.dot(P,np.linalg.inv(T)) 81 | return P 82 | 83 | def align_M2( M, P ): 84 | P = np.dot(P,np.linalg.inv(M)) 85 | return P 86 | -------------------------------------------------------------------------------- /pymvg/calibration.py: -------------------------------------------------------------------------------- 1 | import pymvg.camera_model 2 | import numpy as np 3 | 4 | def create_matrix_A(hom_points_3d, hom_points_2d): 5 | """build matrix A for DLT method""" 6 | 7 | assert hom_points_3d.ndim == 2 and hom_points_3d.shape[1] == 4 8 | assert hom_points_2d.ndim == 2 and hom_points_2d.shape[1] == 3 9 | assert hom_points_3d.shape[0] == hom_points_2d.shape[0] 10 | 11 | N = hom_points_3d.shape[0] 12 | 13 | _A = [] 14 | for i in range(N): 15 | X, Y, Z, S = hom_points_3d[i] 16 | u, v, w = hom_points_2d[i] 17 | 18 | _A.append([ 0, 0, 0, 0, -w*X, -w*Y, -w*Z, -w*S, v*X, v*Y, v*Z, v*S]) 19 | _A.append([w*X, w*Y, w*Z, w*S, 0, 0, 0, 0, -u*X, -u*Y, -u*Z, -u*S]) 20 | 21 | A = np.array(_A, dtype=np.float64) 22 | assert A.shape == (2*N, 12) 23 | return A 24 | 25 | 26 | def get_normalize_2d_matrix(points_2d): 27 | """normalize 2d points to mean 0 and rms sqrt(2)""" 28 | 29 | pts_mean = points_2d.mean(axis=0) 30 | centered_pts_2d = points_2d - pts_mean 31 | s = 1 / np.linalg.norm(centered_pts_2d).mean() 32 | xm, ym = pts_mean 33 | T = np.array([[s, 0, -s*xm], 34 | [0, s, -s*ym], 35 | [0, 0, 1 ]], dtype=np.float64) 36 | return T 37 | 38 | 39 | def get_normalize_3d_matrix(points_3d): 40 | """normalize 3d points to mean 0 and rms sqrt(2)""" 41 | 42 | pts_mean = points_3d.mean(axis=0) 43 | centered_pts_3d = points_3d - pts_mean 44 | s = 1 / np.linalg.norm(centered_pts_3d).mean() 45 | xm, ym, zm = pts_mean 46 | U = np.array([[s, 0, 0, -s*xm], 47 | [0, s, 0, -s*ym], 48 | [0, 0, s, -s*zm], 49 | [0, 0, 0, 1 ]], dtype=np.float64) 50 | return U 51 | 52 | 53 | def get_homogeneous_coordinates(points): 54 | """return points in homogeneous coordinates""" 55 | 56 | assert points.ndim == 2 57 | assert points.shape[1] in [2, 3] 58 | if points.shape[1] == 3: 59 | assert not np.allclose(points[:,2], 1.) 60 | return np.hstack((points, np.ones((points.shape[0], 1)))) 61 | 62 | 63 | def DLT(X3d, x2d, width=640, height=480): 64 | """Given 3D coordinates X3d and 2d coordinates x2d, find camera model""" 65 | 66 | # Note: may want to implement Hatze (1988). This returns 67 | # orthogonal rotation matrix. See notes and implementation in 68 | # https://github.com/NatPRoach/HawkMothCode/blob/master/ManualExtractionCode/DLTcal5.m 69 | 70 | # Implementation of DLT algorithm from 71 | # "Hartley & Zisserman - Multiple View Geometry in computer vision - 2nd Edition" 72 | 73 | # Normalize 2d points and keep transformation matrix 74 | T = get_normalize_2d_matrix(x2d) 75 | Tinv = np.linalg.inv(T) 76 | hom_points_2d = get_homogeneous_coordinates(x2d) 77 | normalized_points_2d = np.empty(hom_points_2d.shape) 78 | for i, x in enumerate(hom_points_2d): 79 | normalized_points_2d[i,:] = np.dot(T, x) 80 | 81 | # Normalize 3d points and keep transformation matrix 82 | U = get_normalize_3d_matrix(X3d) 83 | hom_points_3d = get_homogeneous_coordinates(X3d) 84 | normalized_points_3d = np.empty(hom_points_3d.shape) 85 | for i, x in enumerate(hom_points_3d): 86 | normalized_points_3d[i,:] = np.dot(U, x) 87 | 88 | # get matrix A 89 | A = create_matrix_A(normalized_points_3d, normalized_points_2d) 90 | 91 | # solve via singular value decomposition 92 | _, singular_values, VT = np.linalg.svd(A, full_matrices=False) 93 | sol_idx = np.argmin(singular_values) 94 | assert sol_idx == 11 95 | Pvec_n = VT.T[:,sol_idx] # that's why we need to pick the rows here... 96 | 97 | P_n = Pvec_n.reshape((3, 4)) 98 | 99 | # Denormalize 100 | P = np.dot(Tinv, np.dot(P_n, U)) 101 | 102 | cam = pymvg.camera_model.CameraModel.load_camera_from_M(P,width=width,height=height) 103 | x2d_reproj = cam.project_3d_to_pixel(X3d) 104 | dx = x2d_reproj - np.array(x2d) 105 | reproj_error = np.sqrt(np.sum(dx**2,axis=1)) 106 | mean_reproj_error = np.mean(reproj_error) 107 | results = {'cam':cam, 108 | 'mean_reproj_error':mean_reproj_error, 109 | } 110 | return results 111 | -------------------------------------------------------------------------------- /pymvg/extern/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/pymvg/1e7ad0bdd5242146b3c916ea6dcbce101261928b/pymvg/extern/__init__.py -------------------------------------------------------------------------------- /pymvg/extern/opencv/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/pymvg/1e7ad0bdd5242146b3c916ea6dcbce101261928b/pymvg/extern/opencv/__init__.py -------------------------------------------------------------------------------- /pymvg/extern/opencv/npcv.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv # ubuntu: sudo apt-get install python3-opencv 3 | 4 | # helper functions --------------- 5 | 6 | def numpy2opencv_image(arr): 7 | arr = np.array(arr) 8 | if arr.ndim==1: 9 | arr = arr[:,np.newaxis] 10 | assert arr.ndim==2 11 | if arr.dtype in [np.float32]: 12 | result = cv.CreateMat( arr.shape[0], arr.shape[1], cv.CV_32FC1) 13 | elif arr.dtype in [np.float64, float]: 14 | result = cv.CreateMat( arr.shape[0], arr.shape[1], cv.CV_64FC1) 15 | elif arr.dtype in [np.uint8]: 16 | result = cv.CreateMat( arr.shape[0], arr.shape[1], cv.CV_8UC1) 17 | else: 18 | raise ValueError('unknown numpy dtype "%s"'%arr.dtype) 19 | for i in range(arr.shape[0]): 20 | for j in range(arr.shape[1]): 21 | result[i,j] = arr[i,j] 22 | return result 23 | 24 | def opencv_image2numpy( cvimage ): 25 | pyobj = np.asarray(cvimage) 26 | if pyobj.ndim == 2: 27 | # new OpenCV version 28 | result = pyobj 29 | else: 30 | # old OpenCV, so hack this 31 | width = cvimage.width 32 | height = cvimage.height 33 | assert cvimage.channels == 1 34 | assert cvimage.nChannels == 1 35 | assert cvimage.depth == 32 36 | assert cvimage.origin == 0 37 | result = np.empty( (height,width), dtype=float ) 38 | for i in range(height): 39 | for j in range(width): 40 | result[i,j] = cvimage[i,j] 41 | return result 42 | 43 | def numpy2opencv_pointmat(npdistorted): 44 | src = cv.CreateMat( npdistorted.shape[0], 1, cv.CV_64FC2) 45 | for i in range(npdistorted.shape[0]): 46 | src[i,0] = npdistorted[i,0], npdistorted[i,1] 47 | return src 48 | 49 | def opencv_pointmat2numpy(dst): 50 | assert dst.width==1 51 | assert dst.channels == 2 52 | r = np.empty( (dst.height,2) ) 53 | for i in range(dst.height): 54 | a,b = dst[i,0] 55 | r[i,:] = a,b 56 | return r 57 | 58 | # --------------------- internal tests ----------------------------- 59 | 60 | def test_roundtrip_image(): 61 | orig = np.array( [[100.0,100], 62 | [100,200], 63 | [100,300], 64 | [100,400]] ) 65 | testcv = numpy2opencv_image(orig) 66 | test = opencv_image2numpy( testcv ) 67 | assert orig.shape==test.shape 68 | assert np.allclose( orig, test ) 69 | 70 | def test_roundtrip_pointmat(): 71 | orig = np.array( [[100.0,100], 72 | [100,200], 73 | [100,300], 74 | [100,400]] ) 75 | testcv = numpy2opencv_pointmat(orig) 76 | test = opencv_pointmat2numpy( testcv ) 77 | assert orig.shape==test.shape 78 | assert np.allclose( orig, test ) 79 | 80 | -------------------------------------------------------------------------------- /pymvg/extern/ros/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/pymvg/1e7ad0bdd5242146b3c916ea6dcbce101261928b/pymvg/extern/ros/__init__.py -------------------------------------------------------------------------------- /pymvg/extern/ros/ros_publisher.py: -------------------------------------------------------------------------------- 1 | """ros_publisher - publish camera information to ROS""" 2 | import roslib # ROS is required for this module. See http://ros.org 3 | roslib.load_manifest('tf') 4 | 5 | import rospy 6 | from geometry_msgs.msg import Pose2D 7 | from sensor_msgs.msg import CameraInfo 8 | import tf.broadcaster 9 | 10 | class ROSPublisher: 11 | def __init__(self,cam,updates_per_second=4.0): 12 | self.cam = cam 13 | self.updates_per_second=updates_per_second 14 | self.tf_b = tf.broadcaster.TransformBroadcaster() 15 | 16 | self.frame_id = '/'+cam.name.replace(' ','_').replace(':','_') 17 | rospy.loginfo('sending example data for camera %r'%self.frame_id) 18 | 19 | self.intr_pub = rospy.Publisher(self.frame_id+'/camera_info', 20 | CameraInfo, latch=True) 21 | 22 | rospy.Timer(rospy.Duration(1.0/self.updates_per_second), self.on_timer) # 20 fps 23 | 24 | def on_timer(self,_): 25 | now = rospy.Time.now() 26 | future = now + rospy.Duration(1.0/self.updates_per_second) 27 | 28 | # publish camera intrinsics 29 | intrinsics = self.cam.get_intrinsics_as_bunch() 30 | intrinsic_msg = CameraInfo(**intrinsics.__dict__) 31 | intrinsic_msg.header.stamp = now 32 | intrinsic_msg.header.frame_id = self.frame_id 33 | self.intr_pub.publish( intrinsic_msg ) 34 | 35 | # publish camera transform 36 | translation, rotation = self.cam.get_ROS_tf() 37 | self.tf_b.sendTransform( translation, 38 | rotation, 39 | future, 40 | self.frame_id, 41 | '/map', 42 | ) 43 | -------------------------------------------------------------------------------- /pymvg/extern/ros/rviz_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import roslib 4 | roslib.load_manifest('visualization_msgs') 5 | roslib.load_manifest('geometry_msgs') 6 | 7 | from visualization_msgs.msg import Marker, MarkerArray 8 | from geometry_msgs.msg import Point 9 | 10 | def v3(arr): 11 | assert arr.ndim==1 12 | assert arr.shape[0]==3 13 | return Point(x=arr[0],y=arr[1],z=arr[2]) 14 | 15 | def get_frustum_markers(cam, id_base=0, scale=1.0, 16 | frame_id = '/map', stamp=None): 17 | uv_raw = np.array([[0,0], 18 | [0,cam.height], 19 | [cam.width, cam.height], 20 | [cam.width, 0], 21 | [0,0]]) 22 | pts3d_near = cam.project_pixel_to_3d_ray( uv_raw, distorted=True, distance=0.1*scale) 23 | pts3d_far = cam.project_pixel_to_3d_ray( uv_raw, distorted=True, distance=scale) 24 | 25 | markers = [] 26 | 27 | # ring at near depth 28 | NEAR_RING=0+id_base 29 | near_ring = marker = Marker() 30 | marker.header.frame_id = frame_id 31 | if stamp is not None: 32 | marker.header.stamp = stamp 33 | marker.id = NEAR_RING 34 | marker.type = Marker.LINE_STRIP 35 | marker.action = Marker.ADD 36 | marker.scale.x = 0.01*scale; #line width 37 | marker.color.a = 1.0; 38 | marker.color.r = 0.6; 39 | marker.color.g = 1.0; 40 | marker.color.b = 0.6; 41 | marker.points = [v3(pts3d_near[i]) for i in range( pts3d_near.shape[0] ) ] 42 | markers.append(marker) 43 | 44 | # ring at far depth 45 | FAR_RING = 1 46 | far_ring = marker = Marker() 47 | marker.header.frame_id = frame_id 48 | if stamp is not None: 49 | marker.header.stamp = stamp 50 | marker.id = FAR_RING+id_base 51 | marker.type = Marker.LINE_STRIP 52 | marker.action = Marker.ADD 53 | marker.scale.x = 0.01*scale; #line width 54 | marker.color.a = 1.0; 55 | marker.color.r = 0.6; 56 | marker.color.g = 1.0; 57 | marker.color.b = 0.6; 58 | marker.points = [v3(pts3d_far[i]) for i in range( pts3d_far.shape[0] ) ] 59 | markers.append(marker) 60 | 61 | # connectors 62 | for i in range(len(pts3d_near)-1): 63 | ID = 2+i 64 | pts3d = np.vstack((pts3d_near[i,:],pts3d_far[i,:])) 65 | 66 | marker = Marker() 67 | marker.header.frame_id = frame_id 68 | if stamp is not None: 69 | marker.header.stamp = stamp 70 | marker.id = ID+id_base 71 | marker.type = Marker.LINE_STRIP 72 | marker.action = Marker.ADD 73 | marker.scale.x = 0.01*scale; #line width 74 | marker.color.a = 1.0; 75 | marker.color.r = 0.6; 76 | marker.color.g = 1.0; 77 | marker.color.b = 0.6; 78 | marker.points = [v3(pts3d[i]) for i in range( pts3d.shape[0] ) ] 79 | markers.append(marker) 80 | 81 | n_ids = marker.id - id_base 82 | 83 | marray = MarkerArray(markers) 84 | return {'markers':marray, 'n_ids':n_ids} 85 | 86 | -------------------------------------------------------------------------------- /pymvg/multi_camera_system.py: -------------------------------------------------------------------------------- 1 | import os 2 | import json 3 | from collections import OrderedDict 4 | import warnings 5 | 6 | import numpy as np 7 | 8 | from pymvg.camera_model import CameraModel 9 | from pymvg.util import pretty_json_dump, is_rotation_matrix, normalize_M, \ 10 | parse_radfile, my_rq, center 11 | from pymvg.align import estsimt 12 | 13 | class MultiCameraSystem: 14 | def __init__(self,cameras): 15 | self._cameras=OrderedDict() 16 | for camera in cameras: 17 | self.append(camera) 18 | 19 | def append(self,camera): 20 | assert isinstance(camera, CameraModel) 21 | name = camera.name 22 | if name in self._cameras: 23 | raise ValueError('Cannot create MultiCameraSystem with ' 24 | 'multiple identically-named cameras.') 25 | self._cameras[name]=camera 26 | 27 | @classmethod 28 | def from_dict(cls, d): 29 | cam_dict_list = d['camera_system'] 30 | cams = [CameraModel.from_dict(cd) for cd in cam_dict_list] 31 | return MultiCameraSystem( cameras=cams ) 32 | 33 | def get_pymvg_str( self ): 34 | d = self.to_dict() 35 | d['__pymvg_file_version__']='1.0' 36 | buf = pretty_json_dump(d) 37 | return buf 38 | 39 | def save_to_pymvg_file( self, fname ): 40 | buf = self.get_pymvg_str() 41 | with open(fname,mode='w') as fd: 42 | fd.write(buf) 43 | 44 | @classmethod 45 | def from_pymvg_str(cls, buf): 46 | d = json.loads(buf) 47 | assert d['__pymvg_file_version__']=='1.0' 48 | cam_dict_list = d['camera_system'] 49 | cams = [CameraModel.from_dict(cd) for cd in cam_dict_list] 50 | return MultiCameraSystem( cameras=cams ) 51 | 52 | @classmethod 53 | def from_pymvg_file(cls, fname): 54 | with open(fname,mode='r') as fd: 55 | buf = fd.read() 56 | return MultiCameraSystem.from_pymvg_str(buf) 57 | 58 | @classmethod 59 | def from_mcsc(cls, dirname ): 60 | '''create MultiCameraSystem from output directory of MultiCamSelfCal''' 61 | 62 | # FIXME: This is a bit convoluted because it's been converted 63 | # from multiple layers of internal code. It should really be 64 | # simplified and cleaned up. 65 | 66 | do_normalize_pmat=True 67 | 68 | all_Pmat = {} 69 | all_Res = {} 70 | all_K = {} 71 | all_distortion = {} 72 | 73 | opj = os.path.join 74 | 75 | with open(opj(dirname,'camera_order.txt'),mode='r') as fd: 76 | cam_ids = fd.read().strip().split('\n') 77 | 78 | with open(os.path.join(dirname,'Res.dat'),'r') as res_fd: 79 | for i, cam_id in enumerate(cam_ids): 80 | fname = 'camera%d.Pmat.cal'%(i+1) 81 | pmat = np.loadtxt(opj(dirname,fname)) # 3 rows x 4 columns 82 | if do_normalize_pmat: 83 | pmat_orig = pmat 84 | pmat = normalize_M(pmat) 85 | all_Pmat[cam_id] = pmat 86 | all_Res[cam_id] = map(int,res_fd.readline().split()) 87 | 88 | # load non linear parameters 89 | rad_files = [ f for f in os.listdir(dirname) if f.endswith('.rad') ] 90 | for cam_id_enum, cam_id in enumerate(cam_ids): 91 | filename = os.path.join(dirname, 92 | 'basename%d.rad'%(cam_id_enum+1,)) 93 | if os.path.exists(filename): 94 | K, distortion = parse_radfile(filename) 95 | all_K[cam_id] = K 96 | all_distortion[cam_id] = distortion 97 | else: 98 | if len(rad_files): 99 | raise RuntimeError( 100 | '.rad files present but none named "%s"'%filename) 101 | warnings.warn('no non-linear data (e.g. radial distortion) ' 102 | 'in calibration for %s'%cam_id) 103 | all_K[cam_id] = None 104 | all_distortion[cam_id] = None 105 | 106 | cameras = [] 107 | for cam_id in cam_ids: 108 | w,h = all_Res[cam_id] 109 | Pmat = all_Pmat[cam_id] 110 | M = Pmat[:,:3] 111 | K,R = my_rq(M) 112 | if not is_rotation_matrix(R): 113 | # RQ may return left-handed rotation matrix. Make right-handed. 114 | R2 = -R 115 | K2 = -K 116 | assert np.allclose(np.dot(K2,R2), np.dot(K,R)) 117 | K,R = K2,R2 118 | 119 | P = np.zeros((3,4)) 120 | P[:3,:3] = K 121 | KK = all_K[cam_id] # from rad file or None 122 | distortion = all_distortion[cam_id] 123 | 124 | # (ab)use PyMVG's rectification to do coordinate transform 125 | # for MCSC's undistortion. 126 | 127 | # The intrinsic parameters used for 3D -> 2D. 128 | ex = P[0,0] 129 | bx = P[0,2] 130 | Sx = P[0,3] 131 | ey = P[1,1] 132 | by = P[1,2] 133 | Sy = P[1,3] 134 | 135 | if KK is None: 136 | rect = np.eye(3) 137 | KK = P[:,:3] 138 | else: 139 | # Parameters used to define undistortion coordinates. 140 | fx = KK[0,0] 141 | fy = KK[1,1] 142 | cx = KK[0,2] 143 | cy = KK[1,2] 144 | 145 | rect = np.array([[ ex/fx, 0, (bx+Sx-cx)/fx ], 146 | [ 0, ey/fy, (by+Sy-cy)/fy ], 147 | [ 0, 0, 1 ]]).T 148 | 149 | if distortion is None: 150 | distortion = np.zeros((5,)) 151 | 152 | C = center(Pmat) 153 | rot = R 154 | t = -np.dot(rot, C)[:,0] 155 | 156 | d = {'width':w, 157 | 'height':h, 158 | 'P':P, 159 | 'K':KK, 160 | 'R':rect, 161 | 'translation':t, 162 | 'Q':rot, 163 | 'D':distortion, 164 | 'name':cam_id, 165 | } 166 | cam = CameraModel.from_dict(d) 167 | cameras.append( cam ) 168 | return MultiCameraSystem( cameras=cameras ) 169 | 170 | def __eq__(self, other): 171 | assert isinstance( self, MultiCameraSystem ) 172 | if not isinstance( other, MultiCameraSystem ): 173 | return False 174 | if len(self.get_names()) != len(other.get_names()): 175 | return False 176 | for name in self.get_names(): 177 | if self._cameras[name] != other._cameras[name]: 178 | return False 179 | return True 180 | 181 | def __ne__(self,other): 182 | return not (self==other) 183 | 184 | def get_names(self): 185 | result = list(self._cameras.keys()) 186 | return result 187 | 188 | def get_camera_dict(self): 189 | return self._cameras 190 | 191 | def get_camera(self,name): 192 | return self._cameras[name] 193 | 194 | def to_dict(self): 195 | return {'camera_system': 196 | [self._cameras[name].to_dict() for name in self._cameras]} 197 | 198 | def find3d(self,pts,undistort=True): 199 | """Find 3D coordinate using all data given 200 | 201 | Implements a linear triangulation method to find a 3D 202 | point. For example, see Hartley & Zisserman section 12.2 203 | (p.312). 204 | 205 | By default, this function will undistort 2D points before 206 | finding a 3D point. 207 | """ 208 | # for info on SVD, see Hartley & Zisserman (2003) p. 593 (see 209 | # also p. 587) 210 | # Construct matrices 211 | A=[] 212 | for name,xy in pts: 213 | cam = self._cameras[name] 214 | if undistort: 215 | xy = cam.undistort( [xy] ) 216 | Pmat = cam.get_M() # Pmat is 3 rows x 4 columns 217 | row2 = Pmat[2,:] 218 | x,y = xy[0,:] 219 | A.append( x*row2 - Pmat[0,:] ) 220 | A.append( y*row2 - Pmat[1,:] ) 221 | 222 | # Calculate best point 223 | A=np.array(A) 224 | u,d,vt=np.linalg.svd(A) 225 | X = vt[-1,0:3]/vt[-1,3] # normalize 226 | return X 227 | 228 | def find2d(self,camera_name,xyz,distorted=True): 229 | cam = self._cameras[camera_name] 230 | 231 | xyz = np.array(xyz) 232 | rank1 = xyz.ndim==1 233 | 234 | xyz = np.atleast_2d(xyz) 235 | pix = cam.project_3d_to_pixel( xyz, distorted=distorted ).T 236 | 237 | if rank1: 238 | # convert back to rank1 239 | pix = pix[:,0] 240 | return pix 241 | 242 | def get_aligned_copy(self, other): 243 | """return copy of self that is scaled, translated, and rotated to best match other""" 244 | assert isinstance( other, MultiCameraSystem) 245 | 246 | orig_names = self.get_names() 247 | new_names = other.get_names() 248 | names = set(orig_names).intersection( new_names ) 249 | if len(names) < 3: 250 | raise ValueError('need 3 or more cameras in common to align.') 251 | orig_points = np.array([ self._cameras[name].get_camcenter() for name in names ]).T 252 | new_points = np.array([ other._cameras[name].get_camcenter() for name in names ]).T 253 | 254 | s,R,t = estsimt(orig_points,new_points) 255 | assert is_rotation_matrix(R) 256 | 257 | new_cams = [] 258 | for name in self.get_names(): 259 | orig_cam = self._cameras[name] 260 | new_cam = orig_cam.get_aligned_camera(s,R,t) 261 | new_cams.append( new_cam ) 262 | result = MultiCameraSystem(new_cams) 263 | return result 264 | 265 | def build_example_system(n=6,z=5.0): 266 | base = CameraModel.load_camera_default() 267 | 268 | x = np.linspace(0, 2*n, n) 269 | theta = np.linspace(0, 2*np.pi, n) 270 | cams = [] 271 | for i in range(n): 272 | # cameras are spaced parallel to the x axis 273 | center = np.array( (x[i], 0.0, z) ) 274 | 275 | # cameras are looking at +y 276 | lookat = center + np.array( (0,1,0)) 277 | 278 | # camera up direction rotates around the y axis 279 | up = -np.sin(theta[i]), 0, np.cos(theta[i]) 280 | 281 | cam = base.get_view_camera(center,lookat,up) 282 | cam.name = 'theta: %.0f'%( np.degrees(theta[i]) ) 283 | cams.append(cam) 284 | 285 | system = MultiCameraSystem(cams) 286 | return system 287 | -------------------------------------------------------------------------------- /pymvg/plot_utils.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | import matplotlib.pyplot as plt 4 | 5 | def plot_camera( ax, cam, scale=0.2, show_upper_left=False, axes_size=0.2): 6 | C = cam.get_camcenter() 7 | C.shape=(3,) 8 | ax.plot( [C[0]], [C[1]], [C[2]], 'ko', ms=5 ) 9 | 10 | world_coords = cam.project_camera_frame_to_3d( [[axes_size,0,0], 11 | [0,axes_size,0], 12 | [0,0,axes_size]]) 13 | 14 | for i in range(3): 15 | c = 'rgb'[i] 16 | vv = world_coords[i] 17 | v = np.vstack( ([C],[vv]) ) 18 | ax.plot( v[:,0], v[:,1], v[:,2], c+'-' ) 19 | 20 | if cam.width is None or cam.height is None: 21 | raise ValueError('Camera width/height must be defined to plot.') 22 | 23 | uv_raw = np.array([[0,0], 24 | [0,cam.height], 25 | [cam.width, cam.height], 26 | [cam.width, 0], 27 | [0,0]]) 28 | pts3d_near = cam.project_pixel_to_3d_ray( uv_raw, distorted=True, distance=0.1*scale) 29 | pts3d_far = cam.project_pixel_to_3d_ray( uv_raw, distorted=True, distance=scale) 30 | # ring at near depth 31 | ax.plot( pts3d_near[:,0], pts3d_near[:,1], pts3d_near[:,2], 'k-' ) 32 | # ring at far depth 33 | ax.plot( pts3d_far[:,0], pts3d_far[:,1], pts3d_far[:,2], 'k-' ) 34 | # connectors 35 | for i in range(len(pts3d_near)-1): 36 | pts3d = np.vstack((pts3d_near[i,:],pts3d_far[i,:])) 37 | ax.plot( pts3d[:,0], pts3d[:,1], pts3d[:,2], 'k-' ) 38 | 39 | ax.text( C[0], C[1], C[2], cam.name ) # fails unless using mplot3d 40 | if show_upper_left: 41 | ax.text( pts3d_far[0,0], pts3d_far[0,1], pts3d_far[0,2], 'UL' ) 42 | 43 | def plot_system( ax, system, **kwargs): 44 | camdict = system.get_camera_dict() 45 | for name in camdict: 46 | cam = camdict[name] 47 | plot_camera( ax, cam, **kwargs) 48 | -------------------------------------------------------------------------------- /pymvg/quaternions.py: -------------------------------------------------------------------------------- 1 | import numpy 2 | import math 3 | 4 | _EPS = numpy.finfo(float).eps * 4.0 5 | 6 | # Downloaded on 6 August, 2013 from 7 | # http://www.lfd.uci.edu/~gohlke/code/transformations.py 8 | # 9 | # Author: Christoph Gohlke 10 | # License: BSD 11 | 12 | def quaternion_matrix(quaternion): 13 | """Return homogeneous rotation matrix from quaternion. 14 | 15 | >>> M = quaternion_matrix([1, 0, 0, 0]) 16 | >>> numpy.allclose(M, numpy.identity(4)) 17 | True 18 | >>> M = quaternion_matrix([0, 1, 0, 0]) 19 | >>> numpy.allclose(M, numpy.diag([1, -1, -1, 1])) 20 | True 21 | 22 | """ 23 | q = numpy.array(quaternion, dtype=numpy.float64, copy=True) 24 | n = numpy.dot(q, q) 25 | if n < _EPS: 26 | return numpy.identity(4) 27 | q *= math.sqrt(2.0 / n) 28 | q = numpy.outer(q, q) 29 | return numpy.array([ 30 | [1.0-q[2, 2]-q[3, 3], q[1, 2]-q[3, 0], q[1, 3]+q[2, 0], 0.0], 31 | [ q[1, 2]+q[3, 0], 1.0-q[1, 1]-q[3, 3], q[2, 3]-q[1, 0], 0.0], 32 | [ q[1, 3]-q[2, 0], q[2, 3]+q[1, 0], 1.0-q[1, 1]-q[2, 2], 0.0], 33 | [ 0.0, 0.0, 0.0, 1.0]]) 34 | 35 | 36 | def quaternion_from_matrix(matrix, isprecise=False): 37 | """Return quaternion from rotation matrix. 38 | 39 | If isprecise is True, the input matrix is assumed to be a precise rotation 40 | matrix and a faster algorithm is used. 41 | 42 | >>> q = quaternion_from_matrix(numpy.identity(4), True) 43 | >>> numpy.allclose(q, [1, 0, 0, 0]) 44 | True 45 | >>> q = quaternion_from_matrix(numpy.diag([1, -1, -1, 1])) 46 | >>> numpy.allclose(q, [0, 1, 0, 0]) or numpy.allclose(q, [0, -1, 0, 0]) 47 | True 48 | >>> R = [[-0.545, 0.797, 0.260, 0], [0.733, 0.603, -0.313, 0], 49 | ... [-0.407, 0.021, -0.913, 0], [0, 0, 0, 1]] 50 | >>> q = quaternion_from_matrix(R) 51 | >>> numpy.allclose(q, [0.19069, 0.43736, 0.87485, -0.083611]) 52 | True 53 | >>> R = [[0.395, 0.362, 0.843, 0], [-0.626, 0.796, -0.056, 0], 54 | ... [-0.677, -0.498, 0.529, 0], [0, 0, 0, 1]] 55 | >>> q = quaternion_from_matrix(R) 56 | >>> numpy.allclose(q, [0.82336615, -0.13610694, 0.46344705, -0.29792603]) 57 | True 58 | 59 | """ 60 | M = numpy.array(matrix, dtype=numpy.float64, copy=False)[:4, :4] 61 | if isprecise: 62 | q = numpy.empty((4, )) 63 | t = numpy.trace(M) 64 | if t > M[3, 3]: 65 | q[0] = t 66 | q[3] = M[1, 0] - M[0, 1] 67 | q[2] = M[0, 2] - M[2, 0] 68 | q[1] = M[2, 1] - M[1, 2] 69 | else: 70 | i, j, k = 1, 2, 3 71 | if M[1, 1] > M[0, 0]: 72 | i, j, k = 2, 3, 1 73 | if M[2, 2] > M[i, i]: 74 | i, j, k = 3, 1, 2 75 | t = M[i, i] - (M[j, j] + M[k, k]) + M[3, 3] 76 | q[i] = t 77 | q[j] = M[i, j] + M[j, i] 78 | q[k] = M[k, i] + M[i, k] 79 | q[3] = M[k, j] - M[j, k] 80 | q *= 0.5 / math.sqrt(t * M[3, 3]) 81 | else: 82 | m00 = M[0, 0] 83 | m01 = M[0, 1] 84 | m02 = M[0, 2] 85 | m10 = M[1, 0] 86 | m11 = M[1, 1] 87 | m12 = M[1, 2] 88 | m20 = M[2, 0] 89 | m21 = M[2, 1] 90 | m22 = M[2, 2] 91 | # symmetric matrix K 92 | K = numpy.array([[m00-m11-m22, 0.0, 0.0, 0.0], 93 | [m01+m10, m11-m00-m22, 0.0, 0.0], 94 | [m02+m20, m12+m21, m22-m00-m11, 0.0], 95 | [m21-m12, m02-m20, m10-m01, m00+m11+m22]]) 96 | K /= 3.0 97 | # quaternion is eigenvector of K that corresponds to largest eigenvalue 98 | w, V = numpy.linalg.eigh(K) 99 | q = V[[3, 0, 1, 2], numpy.argmax(w)] 100 | if q[0] < 0.0: 101 | numpy.negative(q, q) 102 | return q 103 | -------------------------------------------------------------------------------- /pymvg/ros_compat.py: -------------------------------------------------------------------------------- 1 | """utilities to emulate parts of ROS 2 | 3 | pymvg was originally written as a [ROS](http://ros.org) package. This 4 | module allows pymvg to run without ROS. 5 | """ 6 | import numpy as np 7 | import json 8 | import io 9 | import warnings 10 | from .quaternions import quaternion_matrix as qquaternion_matrix 11 | from .quaternions import quaternion_from_matrix as qquaternion_from_matrix 12 | 13 | class FakeMessage(object): 14 | """abstract base class""" 15 | _sub_msgs = [] 16 | _sub_attrs = [] 17 | def __init__(self,**kwargs): 18 | for key in self._sub_msgs: 19 | setattr(self,key,self._sub_msgs[key]()) 20 | for key in kwargs: 21 | setattr(self,key,kwargs[key]) 22 | def get_dict(self): 23 | result = {} 24 | result['__json_message__'] = self.get_message_type() 25 | for key in self._sub_msgs: 26 | value = getattr(self,key) 27 | val_simple = value.get_dict() 28 | result[key] = val_simple 29 | for key in self._sub_attrs: 30 | value = getattr(self,key) 31 | result[key] = value 32 | return result 33 | def get_message_type(self): 34 | return self._msg_type 35 | 36 | def make_list(vec): 37 | avec = np.array(vec) 38 | assert avec.ndim==1 39 | result = [] 40 | for el in avec: 41 | newel = float(el) 42 | assert newel==el 43 | result.append(newel) 44 | return result 45 | 46 | class CameraInfo(FakeMessage): 47 | _sub_msgs = [] 48 | _sub_attrs = ['D','P','K','R','width','height'] 49 | _msg_type = 'CameraInfo' 50 | def _get_simple_dict(self): 51 | result = {} 52 | for key in ['D','P','K','R']: 53 | result[key] = make_list(getattr(self,key)) 54 | result['width'] = self.width 55 | result['height'] = self.height 56 | return result 57 | def __str__(self): 58 | d = self._get_simple_dict() 59 | result = json.dumps(d,sort_keys=True,indent=4) 60 | return result 61 | 62 | class Point(FakeMessage): 63 | _msg_type = 'Point' 64 | _sub_attrs = ['x','y','z'] 65 | 66 | class Quaternion(FakeMessage): 67 | _msg_type = 'Quaternion' 68 | _sub_attrs = ['x','y','z','w'] 69 | 70 | class Vector3(FakeMessage): 71 | _msg_type = 'Vector3' 72 | _sub_attrs = ['x','y','z'] 73 | 74 | class Transform(FakeMessage): 75 | _msg_type = 'Transform' 76 | _sub_msgs = {'translation':Vector3,'rotation':Quaternion} 77 | 78 | def make_registry(mlist): 79 | registry = {} 80 | for m in mlist: 81 | registry[m._msg_type] = m 82 | return registry 83 | 84 | registry = make_registry( [Quaternion, Vector3, Transform, CameraInfo] ) 85 | 86 | def strify_message(msg): 87 | assert isinstance(msg,FakeMessage) 88 | return str(msg) 89 | 90 | def fake_message_encapsulate(topic,value): 91 | result = {} 92 | result['__json_toplevel__'] = True 93 | result['topic'] = topic 94 | result['value'] = value.get_dict() 95 | return result 96 | 97 | def fake_message_writer( messages, fd ): 98 | msg_list = [] 99 | for topic,value in messages: 100 | assert isinstance(value,FakeMessage), ('%r not instance of FakeMessage' % (value,)) 101 | sd = fake_message_encapsulate( topic, value ) 102 | msg_list.append(sd) 103 | buf = json.dumps(msg_list, sort_keys=True, indent=4) 104 | try: 105 | # Python 2 106 | str_type = unicode 107 | except NameError: 108 | # Python 3 109 | str_type = bytes 110 | utf8 = str_type(buf.encode('UTF-8')) 111 | fd.write(utf8) 112 | 113 | def parse_json_schema(m): 114 | typ=m['__json_message__'] 115 | klass = registry[ typ ] 116 | result = klass() 117 | for attr in result._sub_attrs: 118 | setattr(result,attr,m[attr]) 119 | for attr in result._sub_msgs: 120 | sub_msg = parse_json_schema( m[attr] ) 121 | setattr(result,attr,sub_msg) 122 | return result 123 | 124 | 125 | def fake_message_loader( fd ): 126 | buf = fd.read() 127 | msg_list = json.loads(buf) 128 | result = [] 129 | for m in msg_list: 130 | assert m['__json_toplevel__'] 131 | topic = m['topic'] 132 | valuebuf = m['value'] 133 | value = parse_json_schema(valuebuf) 134 | result.append( (topic, value) ) 135 | return result 136 | 137 | class Bag(object): 138 | def __init__(self, file, mode): 139 | assert mode in ['r','w'] 140 | self.mode=mode 141 | if hasattr(file,'write'): 142 | self.fd = file 143 | else: 144 | self.fd = io.open(file,mode=mode,encoding='UTF-8') 145 | if mode=='w': 146 | warnings.warn('pymvg.ros_compat.Bag is writing a file, but this ' 147 | 'is not a real bag file.') 148 | self.messages = [] 149 | else: 150 | self.messages = fake_message_loader( self.fd ) 151 | self.closed=False 152 | def __del__(self): 153 | self.close() 154 | def write(self,topic,value): 155 | self.messages.append( (topic,value) ) 156 | def close(self): 157 | if self.closed: 158 | return 159 | if self.mode=='w': 160 | fake_message_writer( self.messages, self.fd ) 161 | self.fd.close() 162 | self.closed = True 163 | 164 | def read_messages(self): 165 | for topic,value in self.messages: 166 | t = 0.0 167 | yield (topic,value,t) 168 | 169 | class Bunch(object): 170 | pass 171 | 172 | def _get_sensor_msgs(): 173 | sensor_msgs = Bunch() 174 | sensor_msgs.msg = Bunch() 175 | sensor_msgs.msg.CameraInfo = CameraInfo 176 | return sensor_msgs 177 | 178 | def _get_geometry_msgs(): 179 | geometry_msgs = Bunch() 180 | geometry_msgs.msg = Bunch() 181 | geometry_msgs.msg.Point = Point 182 | geometry_msgs.msg.Quaternion = Quaternion 183 | geometry_msgs.msg.Transform = Transform 184 | return geometry_msgs 185 | 186 | sensor_msgs = _get_sensor_msgs() 187 | geometry_msgs = _get_geometry_msgs() 188 | -------------------------------------------------------------------------------- /pymvg/util.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | import os, re 4 | import json 5 | import warnings 6 | 7 | from .quaternions import quaternion_matrix, quaternion_from_matrix 8 | 9 | # helper class 10 | 11 | class Bunch: 12 | def __init__(self, **kwds): 13 | self.__dict__.update(kwds) 14 | 15 | # helper functions --------------- 16 | 17 | def is_string(value): 18 | try: 19 | # Python 2 20 | return isinstance(value,basestring) 21 | except: 22 | # Python 3 23 | return isinstance(value,str) 24 | 25 | def point_msg_to_tuple(d): 26 | return d.x, d.y, d.z 27 | 28 | def normalize(vec): 29 | mag = np.sqrt(np.sum(vec**2)) 30 | return vec/mag 31 | 32 | def parse_rotation_msg(rotation, force_matrix=False): 33 | # rotation could either be a quaternion or a 3x3 matrix 34 | 35 | if (hasattr(rotation,'x') and 36 | hasattr(rotation,'y') and 37 | hasattr(rotation,'z') and 38 | hasattr(rotation,'w')): 39 | # convert quaternion message to tuple 40 | rotation = quaternion_msg_to_tuple(rotation) 41 | 42 | if len(rotation)==4: 43 | if force_matrix: 44 | rotation = quaternion_matrix(rotation)[:3,:3] 45 | return rotation 46 | 47 | if len(rotation) != 9: 48 | raise ValueError('expected rotation to be a quaternion or 3x3 matrix') 49 | rotation = np.array( rotation ) 50 | rotation.shape = 3,3 51 | return rotation 52 | 53 | def np2plain(arr): 54 | '''convert numpy array to plain python (for serializing to yaml or json)''' 55 | arr = np.array(arr) 56 | if arr.ndim==1: 57 | result = plain_vec(arr) 58 | elif arr.ndim==2: 59 | result = [ plain_vec(row) for row in arr ] 60 | else: 61 | raise NotImplementedError 62 | return result 63 | 64 | def quaternion_msg_to_tuple(d): 65 | return d.x, d.y, d.z, d.w 66 | 67 | def _undistort( xd, yd, D): 68 | # See OpenCV modules/imgprc/src/undistort.cpp 69 | x = np.array(xd,copy=True) 70 | y = np.array(yd,copy=True) 71 | 72 | t1,t2 = D[2:4] 73 | 74 | k = list(D) 75 | if len(k)==5: 76 | k = k + [0,0,0] 77 | 78 | for i in range(5): 79 | r2 = x*x + y*y 80 | icdist = (1 + ((k[7]*r2 + k[6])*r2 + k[5])*r2)/ \ 81 | (1 + ((k[4]*r2 + k[1])*r2 + k[0])*r2) 82 | delta_x = 2.0 * (t1)*x*y + (t2)*(r2 + 2.0*x*x) 83 | delta_y = (t1) * (r2 + 2.0*y*y)+2.0*(t2)*x*y 84 | x = (xd-delta_x)*icdist 85 | y = (yd-delta_y)*icdist 86 | return x,y 87 | 88 | 89 | def rq(A): 90 | # see first comment at 91 | # http://leohart.wordpress.com/2010/07/23/rq-decomposition-from-qr-decomposition/ 92 | from numpy.linalg import qr 93 | from numpy import flipud 94 | Q,R = qr(flipud(A).T) 95 | R = flipud(R.T) 96 | Q = Q.T 97 | return R[:,::-1],Q[::-1,:] 98 | 99 | def my_rq(M): 100 | """RQ decomposition, ensures diagonal of R is positive""" 101 | # note the side-effects this has: 102 | # http://ksimek.github.io/2012/08/14/decompose/ (TODO: check for 103 | # these effects in PyMVG.) 104 | R,K = rq(M) 105 | n = R.shape[0] 106 | for i in range(n): 107 | if R[i,i]<0: 108 | # I checked this with Mathematica. Works if R is upper-triangular. 109 | R[:,i] = -R[:,i] 110 | K[i,:] = -K[i,:] 111 | return R,K 112 | 113 | def center(P,eps=1e-8): 114 | orig_determinant = np.linalg.det 115 | def determinant( A ): 116 | return orig_determinant( np.asarray( A ) ) 117 | # camera center 118 | X = determinant( [ P[:,1], P[:,2], P[:,3] ] ) 119 | Y = -determinant( [ P[:,0], P[:,2], P[:,3] ] ) 120 | Z = determinant( [ P[:,0], P[:,1], P[:,3] ] ) 121 | T = -determinant( [ P[:,0], P[:,1], P[:,2] ] ) 122 | 123 | assert abs(T)>eps, "cannot calculate 3D camera center: camera at infinity" 124 | C_ = np.array( [[ X/T, Y/T, Z/T ]] ).T 125 | return C_ 126 | 127 | def is_rotation_matrix(R,eps=1e-8): 128 | # check if rotation matrix is really a pure rotation matrix 129 | 130 | # test: inverse is transpose 131 | testI = np.dot(R.T,R) 132 | if not np.allclose( testI, np.eye(len(R)) ): 133 | return False 134 | 135 | # test: determinant is unity 136 | dr = np.linalg.det(R) 137 | if not (abs(dr-1.0)eps: 203 | pmat = pmat/K[2,2] 204 | assert np.allclose(center(pmat_orig),center(pmat)) 205 | return pmat 206 | 207 | def parse_radfile(filename): 208 | result = {} 209 | regex = re.compile(r'^(?P[_a-zA-Z][a-zA-Z0-9_.]*)\s*=\s*(?P.*)$') 210 | with open(filename,mode='r') as fd: 211 | for line in fd.readlines(): 212 | line = line[:line.find('#')] # strip comments 213 | line = line.strip() # strip whitespace 214 | if len(line)==0: 215 | # discard empty lines 216 | continue 217 | matchobj = regex.match(line) 218 | assert matchobj is not None 219 | d = matchobj.groupdict() 220 | result[ d['key'] ] = float(d['value']) 221 | 222 | K = np.zeros((3,3)) 223 | for i in range(3): 224 | for j in range(3): 225 | K[i,j] = result[ 'K%d%d'%(i+1, j+1) ] 226 | 227 | distortion = np.array(( result['kc1'], 228 | result['kc2'], 229 | result['kc3'], 230 | result['kc4'], 231 | result.get('kc5',0.0) )) 232 | return K, distortion 233 | 234 | # JSON compatible pretty printing ---------------- 235 | 236 | def _pretty_vec(row): 237 | els = [ json.dumps(el) for el in row ] 238 | rowstr = '[ ' + ', '.join(els) + ' ]' 239 | return rowstr 240 | 241 | def _pretty_arr(arr,indent=11): 242 | rowstrs = [] 243 | for row in arr: 244 | rowstr = _pretty_vec(row) 245 | rowstrs.append( rowstr ) 246 | sep = ',\n' + ' '*indent 247 | buf = '[' + sep.join(rowstrs) + ']' 248 | return buf 249 | 250 | def _cam_str(cam): 251 | buf = '''{"name": "%s", 252 | "width": %d, 253 | "height": %d, 254 | "P": %s, 255 | "K": %s, 256 | "D": %s, 257 | "R": %s, 258 | "Q": %s, 259 | "translation": %s 260 | }'''%(cam['name'], cam['width'], cam['height'], _pretty_arr(cam['P']), 261 | _pretty_arr(cam['K']), 262 | _pretty_vec(cam['D']), 263 | _pretty_arr(cam['R']), 264 | _pretty_arr(cam['Q']), 265 | _pretty_vec(cam['translation']) 266 | ) 267 | return buf 268 | 269 | def pretty_json_dump(d): 270 | keys = list(d.keys()) 271 | assert len(keys)==2 272 | assert d['__pymvg_file_version__']=='1.0' 273 | cams = d['camera_system'] 274 | cam_strs = [_cam_str(cam) for cam in cams] 275 | cam_strs = ',\n '.join(cam_strs) 276 | buf = '''{ "__pymvg_file_version__": "1.0", 277 | "camera_system": [ 278 | %s 279 | ] 280 | }''' % cam_strs 281 | return buf 282 | 283 | # end pretty printing ---------- 284 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [project] 2 | name = "pymvg" 3 | version = "2.1.0" # keep in sync with pymvg/__init__.py 4 | description = "Python Multi-View Geometry" 5 | readme = "README.md" 6 | license.file = "LICENSE.txt" 7 | authors = [{ name = "Andrew Straw", email = "strawman@astraw.com" }] 8 | requires-python = ">=3.8" 9 | 10 | urls.homepage = "https://github.com/strawlab/pymvg" 11 | 12 | dependencies = [ 13 | "PyYAML >= 5.1", 14 | "numpy", 15 | "matplotlib", 16 | "multicamselfcal >= 0.2.1", 17 | ] 18 | 19 | [build-system] 20 | requires = ["setuptools"] 21 | build-backend = "setuptools.build_meta" 22 | 23 | [tool.pytest.ini_options] 24 | addopts = [ 25 | "--doctest-modules", 26 | "--ignore=examples/plot_cameras.py", 27 | "--ignore=examples/plot_camera_system.py", 28 | "--ignore=tests/external/opencv/test_versus_opencv.py", 29 | "--ignore=tests/external/ros/test_full_ros_pipeline.py", 30 | "--doctest-ignore-import-errors", 31 | ] 32 | -------------------------------------------------------------------------------- /tests/external/mcsc/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/pymvg/1e7ad0bdd5242146b3c916ea6dcbce101261928b/tests/external/mcsc/__init__.py -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/Ce.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: C 3 | # type: matrix 4 | # rows: 3 5 | # columns: 4 6 | -2.149627949431321 -1.298276189779124 -1.019292323904738 -1.929055980256195 7 | 0.2101664456025998 1.910602004069201 1.77856192867468 0.270926379272551 8 | -2.03959144630595 -2.297959932933262 1.243644002876956 1.129055400463282 9 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/Cst.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: Cst 3 | # type: matrix 4 | # rows: 4 5 | # columns: 3 6 | 0.3769911852452785 -0.2414394436690657 0.5400679690782269 7 | 0.4401259383070987 0.1901373916027292 0.5549017262319864 8 | -0.3660009194993624 0.2020353798407441 0.4949725194214379 9 | -0.3436105862019766 -0.1982675982642416 0.511752420486778 10 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/IdMat.dat: -------------------------------------------------------------------------------- 1 | 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 2 | 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 1 1 1 1 1 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 3 | 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 0 0 1 1 1 1 1 1 1 1 0 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 1 1 1 1 1 1 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 4 | 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 0 0 0 0 0 0 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 5 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/Pmatrices.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: P 3 | # type: matrix 4 | # rows: 12 5 | # columns: 4 6 | 521.1837636549863 32.07733194532562 -111.7598036788932 885.6652666789 7 | 181.4475575877324 -429.9839399912527 91.83133353617218 667.7113398640474 8 | 0.7626662705959106 -0.0403219987768007 0.6455341169217697 2.964548925696339 9 | 107.0797611772447 -75.64072607351734 490.8348867573943 1411.457330623291 10 | 450.7950235460258 -50.9170283368223 44.26078385321949 784.2481298172239 11 | 0.3861631082350678 -0.8077193449467293 0.4454969288750005 3.06831066077294 12 | -34.0602370639216 -495.6233535892152 82.08545225929535 744.6944090405013 13 | 429.2666713981012 -182.8308629082913 60.93214011415397 686.9463445842227 14 | 0.3531522639835866 -0.7500913534357018 -0.5591479589019933 2.389430321906207 15 | 99.11577077529888 -209.6470961426575 -474.6880423739257 783.9478968123512 16 | 350.2454294599253 -291.3084652627516 25.64623254245789 725.6101706485501 17 | 0.7798911097514047 0.07392830492787299 -0.6215339593789014 2.186170754642797 18 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/Pst.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: Pst 3 | # type: matrix 4 | # rows: 12 5 | # columns: 3 6 | 0.001774301212841387 0.0002178862129542581 -1.272724359085696 7 | 0.0009417101751935271 -0.002013816526283506 0.4645367865151823 8 | -0.00123138754064369 -0.00120508700312219 -0.02225912523667607 9 | -0.002221811210751469 0.0003707935492023545 0.1666836898982952 10 | 0.00102930803443959 0.001937294348554235 -1.30476525968204 11 | 0.0006757625837622393 -0.001632217090620877 -0.5541200598883159 12 | -0.001670448114205281 -0.001300254053996069 1.401287450558097 13 | -0.001912371437133011 0.001111325096288465 -0.1769835677709351 14 | 4.886551691779118e-05 -0.001860681999049991 -0.2180997798425972 15 | 0.001647332413011785 -0.001158635271899826 0.3027833607807887 16 | -0.001950895706680445 -0.001246758806085117 1.422922499110882 17 | 0.0002629685945454353 -0.001911625336698493 -0.2999673484840217 18 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/Re.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: R 3 | # type: matrix 4 | # rows: 12 5 | # columns: 3 6 | -0.6454798330690737 -0.1109963544788949 0.7556689714375707 7 | -0.04118185032809717 0.9930024902823955 0.1106802127596212 8 | -0.7626662705959106 0.04032199877680069 -0.6455341169217697 9 | 0.03345238858172996 -0.4703793424397874 -0.8818300357234892 10 | -0.9218237312720954 -0.3554331639067173 0.1546230075339775 11 | -0.3861631082350679 0.8077193449467295 -0.4454969288750006 12 | 0.3738826348444785 0.6610100532856098 -0.6505977903567653 13 | -0.8576101992193293 -0.02070437046502101 -0.5138833284303254 14 | -0.3531522639835865 0.7500913534357017 0.5591479589019932 15 | 0.458567123620548 0.6083715607124064 0.6477655727581765 16 | -0.4260117956877484 0.7902016513820017 -0.4405624814802751 17 | -0.7798911097514047 -0.07392830492787299 0.6215339593789014 18 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/Res.dat: -------------------------------------------------------------------------------- 1 | 659 494 2 | 659 494 3 | 659 494 4 | 659 494 5 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/STDERR: -------------------------------------------------------------------------------- 1 | warning: addpath: ./CalTechCal: No such file or directory 2 | warning: The calibration file config.files.CalPmat does not exist 3 | warning: No P mat available 4 | warning: The calibration file config.files.CalPmat does not exist 5 | warning: No P mat available 6 | warning: The calibration file config.files.CalPmat does not exist 7 | warning: No P mat available 8 | warning: The calibration file config.files.CalPmat does not exist 9 | warning: No P mat available 10 | warning: No Pmat available 11 | warning: function name `readradfiles' does not agree with function file name `/mnt/hgfs/straw/Documents/src/MultiCamSelfCal/RadialDistortions/readradfile.m' 12 | warning: load: loaded ASCII file `/mnt/hgfs/straw/Documents/src/MultiCamSelfCal/strawlab/test-data/caldata20130726_122220/result/original_cam_centers.dat' -- ignoring extra args 13 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/STDOUT: -------------------------------------------------------------------------------- 1 | GNU Octave, version 3.2.4 2 | Copyright (C) 2009 John W. Eaton and others. 3 | This is free software; see the source code for copying conditions. 4 | There is ABSOLUTELY NO WARRANTY; not even for MERCHANTABILITY or 5 | FITNESS FOR A PARTICULAR PURPOSE. For details, type `warranty'. 6 | 7 | Octave was configured for "x86_64-pc-linux-gnu". 8 | 9 | Additional information about Octave is available at http://www.octave.org. 10 | 11 | Please contribute if you find this software useful. 12 | For more information, visit http://www.octave.org/help-wanted.html 13 | 14 | Report bugs to (but first, please read 15 | http://www.octave.org/bugs.html to learn how to write a helpful report). 16 | 17 | For information about changes from previous versions, type `news'. 18 | 19 | arg = --config=/mnt/hgfs/straw/Documents/src/MultiCamSelfCal/strawlab/test-data/caldata20130726_122220/result/multicamselfcal.cfg 20 | config_dir = /mnt/hgfs/straw/Documents/src/MultiCamSelfCal/strawlab/test-data/caldata20130726_122220/result/ 21 | Multi-Camera Self-Calibration, Tomas Svoboda et al., 07/2003 22 | ************************************************************ 23 | Experiment name: strawlab_test 24 | 25 | ********** After 0 iteration ******************************************* 26 | RANSAC validation step running with tolerance threshold: 10.00 ... 27 | RANSAC: 4 samples, 87 inliers out of 87 points 28 | RANSAC: 1 samples, 87 inliers out of 87 points 29 | RANSAC: 2 samples, 70 inliers out of 70 points 30 | RANSAC: 1 samples, 59 inliers out of 59 points 31 | 87 points/frames have survived validations so far 32 | Filling of missing points is running ... 33 | Repr. error in proj. space (no fact./fact.) is ... 0.542818 0.436898 34 | ************************************************************ 35 | Number of detected outliers: 0 36 | About cameras (Id, 2D reprojection error, #inliers): 37 | CamId std mean #inliers 38 | 1 0.79 0.40 87 39 | 2 0.38 0.37 70 40 | 3 0.46 0.44 59 41 | 4 0.46 0.53 87 42 | *************************************************************** 43 | ************************************************************** 44 | Refinement by using Bundle Adjustment 45 | Repr. error in proj. space (no fact./fact./BA) is ... 0.512143 0.434507 0.302041 46 | 2D reprojection error 47 | All points: mean 0.30 pixels, std is 0.22 48 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/Xe.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: X 3 | # type: matrix 4 | # rows: 4 5 | # columns: 87 6 | -1.457180832320906 -1.406484289897593 -1.308734073530311 -1.130928260578989 -0.6366482584687526 -0.5487567026124627 -0.8323631370438395 -1.255021513558854 -1.643481616127221 -0.9217799354965829 -1.38230429108901 -1.433537400789021 -0.8327935556499828 -0.1705181069210137 -0.809004236062419 -0.6663628436909911 -1.333798321287389 -1.026383198561834 -0.7698577161484385 -1.514720295623541 -1.393147618636518 -1.349345276269568 -1.601512226911353 -1.508965724029641 -0.5985417414972387 -1.520922585811335 -0.7326870482324362 -0.7876915843930484 -0.9707236987885745 -0.6688743340807327 -0.4869753173487208 -0.465191277226526 -0.3757624993037809 -0.3671470348512932 -0.5857814499724249 -0.5296326647177209 -0.2076583176333669 -0.2597328453725299 -0.2977288621685606 -0.4401933201731357 -0.4980889738702058 -0.4989978238249553 -0.501579677491691 -0.7597073494865411 -0.6245458022907028 0.5879748489491743 0.5717062896634698 0.1217534258344785 0.02601550065741343 -0.5562260416815773 -1.207456029334132 -0.7029629843411694 -1.510951018295253 -0.71272615854812 -1.1395845352225 -1.585082124009907 -1.141617539037536 -1.257365479809376 -0.9013410420444871 -0.9219990587254656 -0.9905763287373736 -0.9946347893220661 -0.9712763926961303 -0.9570022938060038 -0.9329133944753475 -0.9661478049205167 -1.017707572304455 -0.9942263251985552 -0.8643466970504725 -0.9494976301258147 -0.9609006488258344 -1.015966094601513 -0.9991149278104011 -0.9154401428019063 -0.9566426698551216 -0.9455194314853623 -0.946351708415082 -0.9474458619484326 -0.9422407154084301 -0.9128667563886996 -0.9042193261712947 -0.9033468614498555 -0.952705615367979 -0.9343248281355796 -0.9252733592307906 -0.8975685440426971 -0.8865777929340968 7 | 0.1423944963617924 0.2426417966075199 0.3875341087436862 0.7468697924263789 1.560739029310555 1.564323472045972 1.167390652274647 0.626430926840548 0.3545851924430407 -0.3025713465514936 0.7959975275694783 0.3235921938758032 -0.1265331644163764 -0.1828330463110225 -0.2161147277440101 0.9933813460548049 0.5885800096291889 0.07685904964415238 1.278207386042405 0.05300563733064741 0.1913749769201739 0.3410521542343162 0.63901916298309 0.5260237583309005 0.4265829756055411 0.6193509505657208 0.8362477571467926 1.511161646893194 0.02022749963480627 -0.1814766449238601 -0.2090264283078123 -0.2279079878084873 -0.2425490095785042 -0.4146225406058536 -0.4625252468179024 -0.1954188012274773 0.3379208763318625 0.3709478499415914 0.4601621900732724 0.7737615333290363 1.010335212065169 1.138244446422975 1.0600197874299 0.9479396790885853 0.4627977032636137 0.4940186361825508 0.2978055298949788 -0.3529597740597298 -0.185801215480237 0.2722485073494249 0.5289143244340057 0.4161106219446444 0.7512072866138485 0.9646418429622424 1.117570509631049 0.4187499280157491 0.896948299496978 0.9853346406016545 0.8457601625844247 0.8076387239651959 0.8123998331863818 0.7597812923345707 0.7448975150376898 0.7890829872391829 0.7712103369009602 0.7261570958873945 0.6602546374544057 0.5192922074930127 0.7300779337607526 0.8000705722015976 0.7875832514973069 0.7813671322961468 0.6618037727753794 0.5384802479043131 0.5339377335239024 0.5261300466829447 0.5183255701473525 0.5199644039007535 0.5139582593598667 0.51631616543757 0.5168015967646141 0.5175821096537382 0.6077670953156772 0.6091926015231991 0.6079464021562887 0.594511524673037 0.5945770963672871 8 | 0.04803191908105878 0.6633150010260671 0.90405888407793 0.7476303528099181 0.8397233672525964 0.8889872866419281 0.5478918626826831 0.05569556354420617 -1.163664593320107 -0.8631626940337493 -0.8483449716769201 -0.712112708717258 -0.806393536597083 -0.7424935621255029 -0.05991065426429078 -0.2717074387099909 0.4047758397090753 0.6411349847309924 0.5828627379277556 -0.03999444920798394 0.7525118297794477 0.8772734179294012 0.6224725838342682 -0.7354432425706121 -0.483774607055916 0.3570622283851942 0.5007439740280809 0.5917958877685903 0.53077208014695 0.4050113014510989 0.2282592199128579 0.5257869433641581 0.4928814147037955 -0.07011954274665561 -1.196832430942209 -2.06824782223719 -1.892069330304808 -1.072467133010906 -0.0502734667168628 0.5212185238378542 0.3110756558200515 -0.4005147307074129 -1.201759823798674 -2.042851720420531 -1.979118853825589 -0.1080759962041311 0.5303407993969761 0.4736050372228847 -1.581220320442317 -1.850813329404967 0.04940541525134082 -0.5919881966220353 -0.9563902849583429 0.1161137622519563 0.2716275588594043 0.05102847954756225 -0.6089998123993455 -0.1275755635373083 -0.02199636627299936 -0.1296468310636481 -0.4325733127562726 -0.6432884997329296 -0.7435233789029825 -0.7277693828072039 -0.673342778486397 -0.6816290390457163 -0.7676983543165752 -0.8214413777804938 -0.9200199243311332 -0.8076843917571064 -0.7771062448801489 -0.7658364584577901 -0.730791678841946 -0.7329240379248623 -0.6613242317454588 -0.6606833846333462 -0.635742420622026 -0.6343293267883426 -0.6321556605470036 -0.6337457294579107 -0.6487598860059398 -0.6583805426537082 -1.12363439494875 -1.162865931289011 -1.162841150410616 -1.15555869919724 -1.164087132825766 9 | 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 1 10 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/basename1.cal: -------------------------------------------------------------------------------- 1 | C1 = 0.376991 2 | C2 = -0.241439 3 | C3 = 0.540068 4 | 5 | P11 = 0.001774 6 | P12 = 0.000218 7 | P13 = -1.272724 8 | P21 = 0.000942 9 | P22 = -0.002014 10 | P23 = 0.464537 11 | P31 = -0.001231 12 | P32 = -0.001205 13 | P33 = -0.022259 14 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/basename1.rad: -------------------------------------------------------------------------------- 1 | K11 = 422.202325 2 | K12 = 0.000000 3 | K13 = 330.145038 4 | K21 = 0.000000 5 | K22 = 424.180871 6 | K23 = 210.309616 7 | K31 = 0.000000 8 | K32 = 0.000000 9 | K33 = 1.000000 10 | 11 | kc1 = -0.280971 12 | kc2 = 0.074959 13 | kc3 = 0.000404 14 | kc4 = -0.000104 15 | 16 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/basename2.cal: -------------------------------------------------------------------------------- 1 | C1 = 0.440126 2 | C2 = 0.190137 3 | C3 = 0.554902 4 | 5 | P11 = -0.002222 6 | P12 = 0.000371 7 | P13 = 0.166684 8 | P21 = 0.001029 9 | P22 = 0.001937 10 | P23 = -1.304765 11 | P31 = 0.000676 12 | P32 = -0.001632 13 | P33 = -0.554120 14 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/basename2.rad: -------------------------------------------------------------------------------- 1 | K11 = 402.101953 2 | K12 = 0.000000 3 | K13 = 320.832798 4 | K21 = 0.000000 5 | K22 = 403.409910 6 | K23 = 239.706027 7 | K31 = 0.000000 8 | K32 = 0.000000 9 | K33 = 1.000000 10 | 11 | kc1 = -0.293525 12 | kc2 = 0.080576 13 | kc3 = -0.000718 14 | kc4 = -0.001240 15 | 16 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/basename3.cal: -------------------------------------------------------------------------------- 1 | C1 = -0.366001 2 | C2 = 0.202035 3 | C3 = 0.494973 4 | 5 | P11 = -0.001670 6 | P12 = -0.001300 7 | P13 = 1.401287 8 | P21 = -0.001912 9 | P22 = 0.001111 10 | P23 = -0.176984 11 | P31 = 0.000049 12 | P32 = -0.001861 13 | P33 = -0.218100 14 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/basename3.rad: -------------------------------------------------------------------------------- 1 | K11 = 397.684777 2 | K12 = 0.000000 3 | K13 = 313.133191 4 | K21 = 0.000000 5 | K22 = 400.068501 6 | K23 = 258.339857 7 | K31 = 0.000000 8 | K32 = 0.000000 9 | K33 = 1.000000 10 | 11 | kc1 = -0.282840 12 | kc2 = 0.078460 13 | kc3 = 0.000912 14 | kc4 = -0.000127 15 | 16 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/basename4.cal: -------------------------------------------------------------------------------- 1 | C1 = -0.343611 2 | C2 = -0.198268 3 | C3 = 0.511752 4 | 5 | P11 = 0.001647 6 | P12 = -0.001159 7 | P13 = 0.302783 8 | P21 = -0.001951 9 | P22 = -0.001247 10 | P23 = 1.422922 11 | P31 = 0.000263 12 | P32 = -0.001912 13 | P33 = -0.299967 14 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/basename4.rad: -------------------------------------------------------------------------------- 1 | K11 = 389.752453 2 | K12 = 0.000000 3 | K13 = 349.609998 4 | K21 = 0.000000 5 | K22 = 391.514349 6 | K23 = 237.332404 7 | K31 = 0.000000 8 | K32 = 0.000000 9 | K33 = 1.000000 10 | 11 | kc1 = -0.271015 12 | kc2 = 0.063892 13 | kc3 = -0.000953 14 | kc4 = 0.000412 15 | 16 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/cam1.points4cal.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: corresp 3 | # type: matrix 4 | # rows: 87 5 | # columns: 7 6 | -1.457180832320906 0.1423944963617924 0.04803191908105878 1 92.678574 187.19925 1 7 | -1.406484289897593 0.2426417966075199 0.6633150010260671 1 72.785713 166.32993 1 8 | -1.308734073530311 0.3875341087436862 0.90405888407793 1 79.29499800000001 146 1 9 | -1.130928260578989 0.7468697924263789 0.7476303528099181 1 117 95.713081 1 10 | -0.6366482584687526 1.560739029310555 0.8397233672525964 1 189 9.5857992 1 11 | -0.5487567026124627 1.564323472045972 0.8889872866419281 1 195 14.289941 1 12 | -0.8323631370438395 1.167390652274647 0.5478918626826831 1 177 41.449276 1 13 | -1.255021513558854 0.626430926840548 0.05569556354420617 1 139.19429 97.98857099999999 1 14 | -1.643481616127221 0.3545851924430407 -1.163664593320107 1 187.42372 120.59937 1 15 | -0.9217799354965829 -0.3025713465514936 -0.8631626940337493 1 287.6442 318.64249 1 16 | -1.38230429108901 0.7959975275694783 -0.8483449716769201 1 224.38678 15.497283 1 17 | -1.433537400789021 0.3235921938758032 -0.712112708717258 1 171.13327 148.37146 1 18 | -0.8327935556499828 -0.1265331644163764 -0.806393536597083 1 296.7366 273.51376 1 19 | -0.1705181069210137 -0.1828330463110225 -0.7424935621255029 1 369.8172 273.25269 1 20 | -0.809004236062419 -0.2161147277440101 -0.05991065426429078 1 204 260.68024 1 21 | -0.6663628436909911 0.9933813460548049 -0.2717074387099909 1 271 50.082874 1 22 | -1.333798321287389 0.5885800096291889 0.4047758397090753 1 103 108.76949 1 23 | -1.026383198561834 0.07685904964415238 0.6411349847309924 1 124.22346 196.48604 1 24 | -0.7698577161484385 1.278207386042405 0.5828627379277556 1 184.43356 30.013987 1 25 | -1.514720295623541 0.05300563733064741 -0.03999444920798394 1 86 206.48268 1 26 | -1.393147618636518 0.1913749769201739 0.7525118297794477 1 70.33206199999999 174.50763 1 27 | -1.349345276269568 0.3410521542343162 0.8772734179294012 1 74 152.42133 1 28 | -1.601512226911353 0.63901916298309 0.6224725838342682 1 54 97.304878 1 29 | -1.508965724029641 0.5260237583309005 -0.7354432425706121 1 163.75351 86.046989 1 30 | -0.5985417414972387 0.4265829756055411 -0.483774607055916 1 294.65463 152 1 31 | -1.520922585811335 0.6193509505657208 0.3570622283851942 1 77 97.367249 1 32 | -0.7326870482324362 0.8362477571467926 0.5007439740280809 1 185.17578 89.765625 1 33 | -0.7876915843930484 1.511161646893194 0.5917958877685903 1 187.81522 0.50543481 1 34 | -0.9707236987885745 0.02022749963480627 0.53077208014695 1 137.81187 207.22772 1 35 | -0.6688743340807327 -0.1814766449238601 0.4050113014510989 1 184 241.55859 1 36 | -0.4869753173487208 -0.2090264283078123 0.2282592199128579 1 220.64088 250 1 37 | -0.465191277226526 -0.2279079878084873 0.5257869433641581 1 199 245.63759 1 38 | -0.3757624993037809 -0.2425490095785042 0.4928814147037955 1 211.35001 248.355 1 39 | -0.3671470348512932 -0.4146225406058536 -0.07011954274665561 1 260.5817 289.60132 1 40 | -0.5857814499724249 -0.4625252468179024 -1.196832430942209 1 393.50723 361.57971 1 41 | -0.5296326647177209 -0.1954188012274773 -2.06824782223719 1 613.5473 347.9527 1 42 | -0.2076583176333669 0.3379208763318625 -1.892069330304808 1 597.1662 200.41759 1 43 | -0.2597328453725299 0.3709478499415914 -1.072467133010906 1 426.71014 176.81781 1 44 | -0.2977288621685606 0.4601621900732724 -0.0502734667168628 1 280 153.30739 1 45 | -0.4401933201731357 0.7737615333290363 0.5212185238378542 1 217 107.28859 1 46 | -0.4980889738702058 1.010335212065169 0.3110756558200515 1 232.25362 69.702896 1 47 | -0.4989978238249553 1.138244446422975 -0.4005147307074129 1 311.5477 32.427792 1 48 | -0.501579677491691 1.0600197874299 -1.201759823798674 1 440.18182 23.218182 1 49 | -0.7597073494865411 0.9479396790885853 -2.042851720420531 1 632.47235 2.6237981 1 50 | -0.6245458022907028 0.4627977032636137 -1.979118853825589 1 616.12488 156.04817 1 51 | 0.5879748489491743 0.4940186361825508 -0.1080759962041311 1 367.31372 166 1 52 | 0.5717062896634698 0.2978055298949788 0.5303407993969761 1 304 185.28 1 53 | 0.1217534258344785 -0.3529597740597298 0.4736050372228847 1 262.77048 261.82788 1 54 | 0.02601550065741343 -0.185801215480237 -1.581220320442317 1 525.8359400000001 300.44125 1 55 | -0.5562260416815773 0.2722485073494249 -1.850813329404967 1 577.87598 209.9771 1 56 | -1.207456029334132 0.5289143244340057 0.04940541525134082 1 145 117.4757 1 57 | -0.7029629843411694 0.4161106219446444 -0.5919881966220353 1 295.43393 151.31982 1 58 | -1.510951018295253 0.7512072866138485 -0.9563902849583429 1 209.51967 6.8058577 1 59 | -0.71272615854812 0.9646418429622424 0.1161137622519563 1 223 63.347828 1 60 | -1.1395845352225 1.117570509631049 0.2716275588594043 1 155.62784 26.317152 1 61 | -1.585082124009907 0.4187499280157491 0.05102847954756225 1 75 128.02319 1 62 | -1.141617539037536 0.896948299496978 -0.6089998123993455 1 239.83443 25.581966 1 63 | -1.257365479809376 0.9853346406016545 -0.1275755635373083 1 166.33203 24.753399 1 64 | -0.9013410420444871 0.8457601625844247 -0.02199636627299936 1 207.39944 70.78492 1 65 | -0.9219990587254656 0.8076387239651959 -0.1296468310636481 1 214.13921 73.443184 1 66 | -0.9905763287373736 0.8123998331863818 -0.4325733127562726 1 238.77193 60.195908 1 67 | -0.9946347893220661 0.7597812923345707 -0.6432884997329296 1 266.16562 63.692142 1 68 | -0.9712763926961303 0.7448975150376898 -0.7435233789029825 1 285.53531 65.494118 1 69 | -0.9570022938060038 0.7890829872391829 -0.7277693828072039 1 286.57086 57.216534 1 70 | -0.9329133944753475 0.7712103369009602 -0.673342778486397 1 281.31049 64.4496 1 71 | -0.9661478049205167 0.7261570958873945 -0.6816290390457163 1 275.73413 71.767059 1 72 | -1.017707572304455 0.6602546374544057 -0.7676983543165752 1 279.06638 81.028023 1 73 | -0.9942263251985552 0.5192922074930127 -0.8214413777804938 1 288.39218 115.25475 1 74 | -0.8643466970504725 0.7300779337607526 -0.9200199243311332 1 333.7056 71.43092300000001 1 75 | -0.9494976301258147 0.8000705722015976 -0.8076843917571064 1 301.67966 52.689754 1 76 | -0.9609006488258344 0.7875832514973069 -0.7771062448801489 1 294.27792 55.738419 1 77 | -1.015966094601513 0.7813671322961468 -0.7658364584577901 1 282.67511 53.541943 1 78 | -0.9991149278104011 0.6618037727753794 -0.730791678841946 1 276.21378 83.068817 1 79 | -0.9154401428019063 0.5384802479043131 -0.7329240379248623 1 287.30133 115.85526 1 80 | -0.9566426698551216 0.5339377335239024 -0.6613242317454588 1 268.8223 115.75741 1 81 | -0.9455194314853623 0.5261300466829447 -0.6606833846333462 1 270.39261 117.94862 1 82 | -0.946351708415082 0.5183255701473525 -0.635742420622026 1 266.23196 120.0627 1 83 | -0.9474458619484326 0.5199644039007535 -0.6343293267883426 1 265.80792 119.59524 1 84 | -0.9422407154084301 0.5139582593598667 -0.6321556605470036 1 266.27539 121.28915 1 85 | -0.9128667563886996 0.51631616543757 -0.6337457294579107 1 271.41623 121.90052 1 86 | -0.9042193261712947 0.5168015967646141 -0.6487598860059398 1 275.23862 122.08321 1 87 | -0.9033468614498555 0.5175821096537382 -0.6583805426537082 1 276.78055 121.85861 1 88 | -0.952705615367979 0.6077670953156772 -1.12363439494875 1 358.49945 90.461624 1 89 | -0.9343248281355796 0.6091926015231991 -1.162865931289011 1 370.44165 90.72534899999999 1 90 | -0.9252733592307906 0.6079464021562887 -1.162841150410616 1 371.75629 91.741783 1 91 | -0.8975685440426971 0.594511524673037 -1.15555869919724 1 374.10129 97.455223 1 92 | -0.8865777929340968 0.5945770963672871 -1.164087132825766 1 377.65237 98.14368399999999 1 93 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/cam2.points4cal.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: corresp 3 | # type: matrix 4 | # rows: 70 5 | # columns: 7 6 | -1.457180832320906 0.1423944963617924 0.04803191908105878 1 500.48572 74 1 7 | -1.406484289897593 0.2426417966075199 0.6633150010260671 1 553 91.699997 1 8 | -1.308734073530311 0.3875341087436862 0.90405888407793 1 579 109 1 9 | -1.130928260578989 0.7468697924263789 0.7476303528099181 1 607 138.75 1 10 | -0.8323631370438395 1.167390652274647 0.5478918626826831 1 640.73212 195.26785 1 11 | -1.255021513558854 0.626430926840548 0.05569556354420617 1 556 112.56579 1 12 | -1.643481616127221 0.3545851924430407 -1.163664593320107 1 383.43576 12.698324 1 13 | -0.9217799354965829 -0.3025713465514936 -0.8631626940337493 1 353.66129 136.43549 1 14 | -1.38230429108901 0.7959975275694783 -0.8483449716769201 1 496.23077 76.25174699999999 1 15 | -1.433537400789021 0.3235921938758032 -0.712112708717258 1 443.71796 64 1 16 | -0.8327935556499828 -0.1265331644163764 -0.806393536597083 1 375 154 1 17 | -0.1705181069210137 -0.1828330463110225 -0.7424935621255029 1 370 242.62 1 18 | -0.809004236062419 -0.2161147277440101 -0.05991065426429078 1 445.83871 151.70967 1 19 | -0.6663628436909911 0.9933813460548049 -0.2717074387099909 1 564.51562 224.98438 1 20 | -1.333798321287389 0.5885800096291889 0.4047758397090753 1 575 104.4 1 21 | -1.026383198561834 0.07685904964415238 0.6411349847309924 1 529 133 1 22 | -0.7698577161484385 1.278207386042405 0.5828627379277556 1 653.8158 208.39473 1 23 | -1.514720295623541 0.05300563733064741 -0.03999444920798394 1 484 63.384617 1 24 | -1.601512226911353 0.63901916298309 0.6224725838342682 1 597 73.583336 1 25 | -1.508965724029641 0.5260237583309005 -0.7354432425706121 1 470 51 1 26 | -0.5985417414972387 0.4265829756055411 -0.483774607055916 1 466.67947 208.51282 1 27 | -1.520922585811335 0.6193509505657208 0.3570622283851942 1 579.2622699999999 78.54098500000001 1 28 | -0.7326870482324362 0.8362477571467926 0.5007439740280809 1 595 194.64102 1 29 | -0.9707236987885745 0.02022749963480627 0.53077208014695 1 516 138 1 30 | -0.6688743340807327 -0.1814766449238601 0.4050113014510989 1 483.60001 167 1 31 | -0.4869753173487208 -0.2090264283078123 0.2282592199128579 1 465 188 1 32 | -0.5857814499724249 -0.4625252468179024 -1.196832430942209 1 296.63159 183 1 33 | -0.5296326647177209 -0.1954188012274773 -2.06824782223719 1 174.82759 221.55173 1 34 | -0.2076583176333669 0.3379208763318625 -1.892069330304808 1 234.67088 313.45569 1 35 | -0.2597328453725299 0.3709478499415914 -1.072467133010906 1 378.2449 274.06122 1 36 | -0.501579677491691 1.0600197874299 -1.201759823798674 1 457.88785 301.28036 1 37 | -0.7597073494865411 0.9479396790885853 -2.042851720420531 1 233.65744 275.32526 1 38 | -0.6245458022907028 0.4627977032636137 -1.979118853825589 1 217.47223 248.91667 1 39 | 0.02601550065741343 -0.185801215480237 -1.581220320442317 1 259 291 1 40 | -0.5562260416815773 0.2722485073494249 -1.850813329404967 1 234.63158 241.97368 1 41 | -0.7029629843411694 0.4161106219446444 -0.5919881966220353 1 456 193 1 42 | -1.510951018295253 0.7512072866138485 -0.9563902849583429 1 475.24139 44 1 43 | -0.71272615854812 0.9646418429622424 0.1161137622519563 1 589 208.57143 1 44 | -1.585082124009907 0.4187499280157491 0.05102847954756225 1 534 59 1 45 | -1.141617539037536 0.896948299496978 -0.6089998123993455 1 536.20514 134.48718 1 46 | -1.257365479809376 0.9853346406016545 -0.1275755635373083 1 594 121 1 47 | -0.9013410420444871 0.8457601625844247 -0.02199636627299936 1 570.54547 174.84848 1 48 | -0.9219990587254656 0.8076387239651959 -0.1296468310636481 1 558.25 170.6579 1 49 | -0.9905763287373736 0.8123998331863818 -0.4325733127562726 1 535.49152 160.37288 1 50 | -0.9946347893220661 0.7597812923345707 -0.6432884997329296 1 505.55945 157.69231 1 51 | -0.9712763926961303 0.7448975150376898 -0.7435233789029825 1 490.71317 162.18605 1 52 | -0.9570022938060038 0.7890829872391829 -0.7277693828072039 1 499 167 1 53 | -0.9329133944753475 0.7712103369009602 -0.673342778486397 1 502 170.54808 1 54 | -0.9661478049205167 0.7261570958873945 -0.6816290390457163 1 495 162 1 55 | -1.017707572304455 0.6602546374544057 -0.7676983543165752 1 475.79739 149.18954 1 56 | -0.9942263251985552 0.5192922074930127 -0.8214413777804938 1 447.81653 148.22935 1 57 | -0.8643466970504725 0.7300779337607526 -0.9200199243311332 1 461.61972 185 1 58 | -0.9494976301258147 0.8000705722015976 -0.8076843917571064 1 490.71234 170 1 59 | -0.9609006488258344 0.7875832514973069 -0.7771062448801489 1 493 166.66013 1 60 | -1.015966094601513 0.7813671322961468 -0.7658364584577901 1 495 155 1 61 | -0.9991149278104011 0.6618037727753794 -0.730791678841946 1 480.26514 153 1 62 | -0.9154401428019063 0.5384802479043131 -0.7329240379248623 1 460 163.44167 1 63 | -0.9566426698551216 0.5339377335239024 -0.6613242317454588 1 469.27612 155.41045 1 64 | -0.9455194314853623 0.5261300466829447 -0.6606833846333462 1 468 157 1 65 | -0.946351708415082 0.5183255701473525 -0.635742420622026 1 470 156.4537 1 66 | -0.9474458619484326 0.5199644039007535 -0.6343293267883426 1 470.21848 156.23529 1 67 | -0.9422407154084301 0.5139582593598667 -0.6321556605470036 1 469.70535 157 1 68 | -0.9128667563886996 0.51631616543757 -0.6337457294579107 1 469 162.33 1 69 | -0.9042193261712947 0.5168015967646141 -0.6487598860059398 1 467.24326 164 1 70 | -0.9033468614498555 0.5175821096537382 -0.6583805426537082 1 466 164.243 1 71 | -0.952705615367979 0.6077670953156772 -1.12363439494875 1 414 162.23129 1 72 | -0.9343248281355796 0.6091926015231991 -1.162865931289011 1 407.22403 166.80328 1 73 | -0.9252733592307906 0.6079464021562887 -1.162841150410616 1 406.82123 168.77095 1 74 | -0.8975685440426971 0.594511524673037 -1.15555869919724 1 405.58823 174 1 75 | -0.8865777929340968 0.5945770963672871 -1.164087132825766 1 403.8208 176.30637 1 76 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/cam3.points4cal.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: corresp 3 | # type: matrix 4 | # rows: 59 5 | # columns: 7 6 | -0.6366482584687526 1.560739029310555 0.8397233672525964 1 132.1875 336.15625 1 7 | -0.5487567026124627 1.564323472045972 0.8889872866419281 1 136.01482 402.05927 1 8 | -1.433537400789021 0.3235921938758032 -0.712112708717258 1 285.3913 17.173914 1 9 | -0.8327935556499828 -0.1265331644163764 -0.806393536597083 1 292 120 1 10 | -0.7698577161484385 1.278207386042405 0.5828627379277556 1 224.21576 191.94521 1 11 | -1.514720295623541 0.05300563733064741 -0.03999444920798394 1 406 41 1 12 | -1.393147618636518 0.1913749769201739 0.7525118297794477 1 531.5 101.78571 1 13 | -1.349345276269568 0.3410521542343162 0.8772734179294012 1 549.78259 112.27174 1 14 | -0.7876915843930484 1.511161646893194 0.5917958877685903 1 126.36842 175.62201 1 15 | -0.9707236987885745 0.02022749963480627 0.53077208014695 1 459.48749 176.66251 1 16 | -0.6688743340807327 -0.1814766449238601 0.4050113014510989 1 428.5625 223 1 17 | -0.4869753173487208 -0.2090264283078123 0.2282592199128579 1 392 236 1 18 | -0.465191277226526 -0.2279079878084873 0.5257869433641581 1 433 267 1 19 | -0.3757624993037809 -0.2425490095785042 0.4928814147037955 1 422 277.32309 1 20 | -0.3671470348512932 -0.4146225406058536 -0.07011954274665561 1 366.33334 230.52632 1 21 | -0.5857814499724249 -0.4625252468179024 -1.196832430942209 1 280.64285 143 1 22 | -0.5296326647177209 -0.1954188012274773 -2.06824782223719 1 203 114.64706 1 23 | -0.2076583176333669 0.3379208763318625 -1.892069330304808 1 150 144 1 24 | -0.2597328453725299 0.3709478499415914 -1.072467133010906 1 189.27777 172.77777 1 25 | -0.2977288621685606 0.4601621900732724 -0.0502734667168628 1 265.77777 239.76389 1 26 | -0.4401933201731357 0.7737615333290363 0.5212185238378542 1 307.66101 284.90396 1 27 | -0.4980889738702058 1.010335212065169 0.3110756558200515 1 224.7583 239.99052 1 28 | -0.4989978238249553 1.138244446422975 -0.4005147307074129 1 122.92593 161.2963 1 29 | -0.501579677491691 1.0600197874299 -1.201759823798674 1 98.428574 119 1 30 | 0.5879748489491743 0.4940186361825508 -0.1080759962041311 1 210.34285 363.74286 1 31 | 0.5717062896634698 0.2978055298949788 0.5303407993969761 1 300.54544 429.68182 1 32 | 0.1217534258344785 -0.3529597740597298 0.4736050372228847 1 390.85367 340.56097 1 33 | -1.207456029334132 0.5289143244340057 0.04940541525134082 1 340.82278 64.316452 1 34 | -0.7029629843411694 0.4161106219446444 -0.5919881966220353 1 241 131.72093 1 35 | -0.71272615854812 0.9646418429622424 0.1161137622519563 1 225.70921 160.03546 1 36 | -1.1395845352225 1.117570509631049 0.2716275588594043 1 258.72043 35.462364 1 37 | -1.585082124009907 0.4187499280157491 0.05102847954756225 1 387.39026 0 1 38 | -1.141617539037536 0.896948299496978 -0.6089998123993455 1 193 32 1 39 | -1.257365479809376 0.9853346406016545 -0.1275755635373083 1 237.8158 8 1 40 | -0.9013410420444871 0.8457601625844247 -0.02199636627299936 1 248.04456 107.79703 1 41 | -0.9219990587254656 0.8076387239651959 -0.1296468310636481 1 242.98204 98.730537 1 42 | -0.9905763287373736 0.8123998331863818 -0.4325733127562726 1 213.24445 70.459259 1 43 | -0.9946347893220661 0.7597812923345707 -0.6432884997329296 1 202.98112 65.76415299999999 1 44 | -1.017707572304455 0.6602546374544057 -0.7676983543165752 1 209.33333 63.151516 1 45 | -0.9942263251985552 0.5192922074930127 -0.8214413777804938 1 223.5 71.76667 1 46 | -0.8643466970504725 0.7300779337607526 -0.9200199243311332 1 178.64706 81 1 47 | -0.9494976301258147 0.8000705722015976 -0.8076843917571064 1 180.8806 67.014923 1 48 | -0.9609006488258344 0.7875832514973069 -0.7771062448801489 1 185.77272 66.439392 1 49 | -1.015966094601513 0.7813671322961468 -0.7658364584577901 1 190.8 57.48 1 50 | -0.9991149278104011 0.6618037727753794 -0.730791678841946 1 210.95744 67 1 51 | -0.9154401428019063 0.5384802479043131 -0.7329240379248623 1 223.6842 86.40351099999999 1 52 | -0.9566426698551216 0.5339377335239024 -0.6613242317454588 1 233.16949 82.389832 1 53 | -0.9455194314853623 0.5261300466829447 -0.6606833846333462 1 233.57747 84.633804 1 54 | -0.946351708415082 0.5183255701473525 -0.635742420622026 1 237 85.74073799999999 1 55 | -0.9474458619484326 0.5199644039007535 -0.6343293267883426 1 237 85.57692 1 56 | -0.9422407154084301 0.5139582593598667 -0.6321556605470036 1 237.74243 86.757576 1 57 | -0.9128667563886996 0.51631616543757 -0.6337457294579107 1 235.47223 91.541664 1 58 | -0.9042193261712947 0.5168015967646141 -0.6487598860059398 1 233.48611 92.402779 1 59 | -0.9033468614498555 0.5175821096537382 -0.6583805426537082 1 232.43939 92.121216 1 60 | -0.952705615367979 0.6077670953156772 -1.12363439494875 1 187.17308 66.653847 1 61 | -0.9343248281355796 0.6091926015231991 -1.162865931289011 1 183.59575 68.340424 1 62 | -0.9252733592307906 0.6079464021562887 -1.162841150410616 1 183.24529 69.73584700000001 1 63 | -0.8975685440426971 0.594511524673037 -1.15555869919724 1 184.21622 74.29729500000001 1 64 | -0.8865777929340968 0.5945770963672871 -1.164087132825766 1 182.95653 75.847824 1 65 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/cam4.points4cal.dat: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: corresp 3 | # type: matrix 4 | # rows: 87 5 | # columns: 7 6 | -1.457180832320906 0.1423944963617924 0.04803191908105878 1 550.75 175.39999 1 7 | -1.406484289897593 0.2426417966075199 0.6633150010260671 1 400.94037 257.87277 1 8 | -1.308734073530311 0.3875341087436862 0.90405888407793 1 231.16611 279.43323 1 9 | -1.130928260578989 0.7468697924263789 0.7476303528099181 1 190.07307 152.21503 1 10 | -0.6366482584687526 1.560739029310555 0.8397233672525964 1 71.504425 92.752213 1 11 | -0.5487567026124627 1.564323472045972 0.8889872866419281 1 62.678261 110.10435 1 12 | -0.8323631370438395 1.167390652274647 0.5478918626826831 1 173.05263 99.467102 1 13 | -1.255021513558854 0.626430926840548 0.05569556354420617 1 408.93848 92.523079 1 14 | -1.643481616127221 0.3545851924430407 -1.163664593320107 1 601 56.416668 1 15 | -0.9217799354965829 -0.3025713465514936 -0.8631626940337493 1 566.375 236.6875 1 16 | -1.38230429108901 0.7959975275694783 -0.8483449716769201 1 497.93478 25.630434 1 17 | -1.433537400789021 0.3235921938758032 -0.712112708717258 1 561.73468 94.020409 1 18 | -0.8327935556499828 -0.1265331644163764 -0.806393536597083 1 534.5625 223 1 19 | -0.1705181069210137 -0.1828330463110225 -0.7424935621255029 1 460.45456 279 1 20 | -0.809004236062419 -0.2161147277440101 -0.05991065426429078 1 486.72726 315.68182 1 21 | -0.6663628436909911 0.9933813460548049 -0.2717074387099909 1 335 106.96154 1 22 | -1.333798321287389 0.5885800096291889 0.4047758397090753 1 358.24506 107.7668 1 23 | -1.026383198561834 0.07685904964415238 0.6411349847309924 1 363.8421 359.17545 1 24 | -0.7698577161484385 1.278207386042405 0.5828627379277556 1 151 93.82894899999999 1 25 | -1.514720295623541 0.05300563733064741 -0.03999444920798394 1 588.09882 180.12791 1 26 | -1.393147618636518 0.1913749769201739 0.7525118297794477 1 384.05267 310.52426 1 27 | -1.349345276269568 0.3410521542343162 0.8772734179294012 1 265.6839 286.03647 1 28 | -1.601512226911353 0.63901916298309 0.6224725838342682 1 329.76425 14.393909 1 29 | -1.508965724029641 0.5260237583309005 -0.7354432425706121 1 545.54547 49.545456 1 30 | -0.5985417414972387 0.4265829756055411 -0.483774607055916 1 421 186 1 31 | -1.520922585811335 0.6193509505657208 0.3570622283851942 1 401.16666 42.372223 1 32 | -0.7326870482324362 0.8362477571467926 0.5007439740280809 1 223.37314 176.7612 1 33 | -0.7876915843930484 1.511161646893194 0.5917958877685903 1 128.7482 55.762589 1 34 | -0.9707236987885745 0.02022749963480627 0.53077208014695 1 390.80054 353.98935 1 35 | -0.6688743340807327 -0.1814766449238601 0.4050113014510989 1 400.07266 388.58548 1 36 | -0.4869753173487208 -0.2090264283078123 0.2282592199128579 1 404.64355 371.78217 1 37 | -0.465191277226526 -0.2279079878084873 0.5257869433641581 1 361.60999 421.01999 1 38 | -0.3757624993037809 -0.2425490095785042 0.4928814147037955 1 358.78818 419.0394 1 39 | -0.3671470348512932 -0.4146225406058536 -0.07011954274665561 1 448.5 367.27048 1 40 | -0.5857814499724249 -0.4625252468179024 -1.196832430942209 1 552.60712 254.76785 1 41 | -0.5296326647177209 -0.1954188012274773 -2.06824782223719 1 555.53333 183.36667 1 42 | -0.2076583176333669 0.3379208763318625 -1.892069330304808 1 486.60001 159.8 1 43 | -0.2597328453725299 0.3709478499415914 -1.072467133010906 1 442.64706 187.31372 1 44 | -0.2977288621685606 0.4601621900732724 -0.0502734667168628 1 337.65625 241.11719 1 45 | -0.4401933201731357 0.7737615333290363 0.5212185238378542 1 214.5415 228.37549 1 46 | -0.4980889738702058 1.010335212065169 0.3110756558200515 1 228.15349 160.8186 1 47 | -0.4989978238249553 1.138244446422975 -0.4005147307074129 1 322.67328 102.40594 1 48 | -0.501579677491691 1.0600197874299 -1.201759823798674 1 410.31708 87.487808 1 49 | -0.7597073494865411 0.9479396790885853 -2.042851720420531 1 489 61.684212 1 50 | -0.6245458022907028 0.4627977032636137 -1.979118853825589 1 513.59088 117.77273 1 51 | 0.5879748489491743 0.4940186361825508 -0.1080759962041311 1 287.77777 285.33334 1 52 | 0.5717062896634698 0.2978055298949788 0.5303407993969761 1 233.34883 359.83722 1 53 | 0.1217534258344785 -0.3529597740597298 0.4736050372228847 1 330.6329 433.78482 1 54 | 0.02601550065741343 -0.185801215480237 -1.581220320442317 1 491 235.4375 1 55 | -0.5562260416815773 0.2722485073494249 -1.850813329404967 1 517.48279 144.51724 1 56 | -1.207456029334132 0.5289143244340057 0.04940541525134082 1 420.54111 123.22546 1 57 | -0.7029629843411694 0.4161106219446444 -0.5919881966220353 1 443.73404 170.23404 1 58 | -1.510951018295253 0.7512072866138485 -0.9563902849583429 1 526.28381 15.891891 1 59 | -0.71272615854812 0.9646418429622424 0.1161137622519563 1 281.95947 125.01351 1 60 | -1.1395845352225 1.117570509631049 0.2716275588594043 1 263.36792 29.68239 1 61 | -1.585082124009907 0.4187499280157491 0.05102847954756225 1 520.7077 72.654945 1 62 | -1.141617539037536 0.896948299496978 -0.6089998123993455 1 435.50476 45.961906 1 63 | -1.257365479809376 0.9853346406016545 -0.1275755635373083 1 375.51654 19.23967 1 64 | -0.9013410420444871 0.8457601625844247 -0.02199636627299936 1 338.68594 108.16942 1 65 | -0.9219990587254656 0.8076387239651959 -0.1296468310636481 1 363.43961 106.01933 1 66 | -0.9905763287373736 0.8123998331863818 -0.4325733127562726 1 410.46155 83.03076900000001 1 67 | -0.9946347893220661 0.7597812923345707 -0.6432884997329296 1 439.8812 84.247528 1 68 | -0.9712763926961303 0.7448975150376898 -0.7435233789029825 1 448.3291 86.632912 1 69 | -0.9570022938060038 0.7890829872391829 -0.7277693828072039 1 439.71429 82.59523799999999 1 70 | -0.9329133944753475 0.7712103369009602 -0.673342778486397 1 434.42267 89.587631 1 71 | -0.9661478049205167 0.7261570958873945 -0.6816290390457163 1 444.43182 91.625 1 72 | -1.017707572304455 0.6602546374544057 -0.7676983543165752 1 465.86047 91.965118 1 73 | -0.9942263251985552 0.5192922074930127 -0.8214413777804938 1 484.29333 113.18667 1 74 | -0.8643466970504725 0.7300779337607526 -0.9200199243311332 1 453.21054 95.736839 1 75 | -0.9494976301258147 0.8000705722015976 -0.8076843917571064 1 444.8085 79.936172 1 76 | -0.9609006488258344 0.7875832514973069 -0.7771062448801489 1 444.8302 80.962265 1 77 | -1.015966094601513 0.7813671322961468 -0.7658364584577901 1 450.54901 75.490196 1 78 | -0.9991149278104011 0.6618037727753794 -0.730791678841946 1 460.42188 95 1 79 | -0.9154401428019063 0.5384802479043131 -0.7329240379248623 1 465.81818 122.49091 1 80 | -0.9566426698551216 0.5339377335239024 -0.6613242317454588 1 465.4592 121.27551 1 81 | -0.9455194314853623 0.5261300466829447 -0.6606833846333462 1 465.06097 123.95122 1 82 | -0.946351708415082 0.5183255701473525 -0.635742420622026 1 464.01163 126.04651 1 83 | -0.9474458619484326 0.5199644039007535 -0.6343293267883426 1 463.66217 125.66216 1 84 | -0.9422407154084301 0.5139582593598667 -0.6321556605470036 1 463.69766 127.27907 1 85 | -0.9128667563886996 0.51631616543757 -0.6337457294579107 1 459.98001 130.33 1 86 | -0.9042193261712947 0.5168015967646141 -0.6487598860059398 1 460.31946 130.59723 1 87 | -0.9033468614498555 0.5175821096537382 -0.6583805426537082 1 460.78946 130.0921 1 88 | -0.952705615367979 0.6077670953156772 -1.12363439494875 1 489.61819 96.618179 1 89 | -0.9343248281355796 0.6091926015231991 -1.162865931289011 1 490.01694 97.30508399999999 1 90 | -0.9252733592307906 0.6079464021562887 -1.162841150410616 1 489.27585 98.327583 1 91 | -0.8975685440426971 0.594511524673037 -1.15555869919724 1 487.32758 102.87931 1 92 | -0.8865777929340968 0.5945770963672871 -1.164087132825766 1 486.64001 103.62 1 93 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/camera1.Pmat.cal: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: Pmat 3 | # type: matrix 4 | # rows: 3 5 | # columns: 4 6 | 478.7925450149012 1218.370598949545 -1949.446427346704 1166.495723405293 7 | -436.3720128267079 -1272.311219057084 -1601.773403989744 722.3887986366142 8 | -2.862328713426669 1.480711314067031 -2.991935743378308 3.052423470678026 9 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/camera2.Pmat.cal: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: Pmat 3 | # type: matrix 4 | # rows: 3 5 | # columns: 4 6 | -2165.972183516988 -45.03479189592399 -545.4998396625285 1264.562140167962 7 | -210.5357165928674 756.3373678262476 -1844.249407686302 972.2313954498842 8 | -2.021300157419557 -2.282789772358982 -3.168647610676592 3.081958350263594 9 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/camera3.Pmat.cal: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: Pmat 3 | # type: matrix 4 | # rows: 3 5 | # columns: 4 6 | -391.6619536633655 -1980.565747600057 -909.2324093930271 706.8407743591273 7 | -291.6697774112099 202.6852037683694 -2038.44840930864 861.2749560120018 8 | 2.400579867852326 -2.172923235015444 -2.975354107431539 2.790340328843046 9 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/camera4.Pmat.cal: -------------------------------------------------------------------------------- 1 | # Created by Octave 3.2.4, Wed Aug 14 07:33:40 2013 CEST 2 | # name: Pmat 3 | # type: matrix 4 | # rows: 3 5 | # columns: 4 6 | 2070.932838476386 -620.0326420263406 -850.8072396114769 1024.064728072647 7 | -141.2407754817732 -384.035092413608 -1964.272221963156 880.5475230036407 8 | 2.715594704006314 1.903814209377727 -2.887585152261646 2.788300449996829 9 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/camera_order.txt: -------------------------------------------------------------------------------- 1 | Basler_21275576 2 | Basler_21275577 3 | Basler_21283674 4 | Basler_21283677 5 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/multicamselfcal.cfg: -------------------------------------------------------------------------------- 1 | [Files] 2 | Basename: basename 3 | Image-Extension: jpg 4 | 5 | [Images] 6 | Subpix: 0.5 7 | 8 | [Calibration] 9 | Num-Cameras: 4 10 | Num-Projectors: 0 11 | Nonlinear-Parameters: 50 0 1 0 0 0 12 | Nonlinear-Update: 1 0 1 0 0 0 13 | Initial-Tolerance: 10 14 | Do-Global-Iterations: 0 15 | Global-Iteration-Threshold: 0.5 16 | Global-Iteration-Max: 5 17 | Num-Cameras-Fill: 4 18 | Do-Bundle-Adjustment: 1 19 | Undo-Radial: 1 20 | Min-Points-Value: 30 21 | N-Tuples: 3 22 | Square-Pixels: 1 23 | Use-Nth-Frame: 5 24 | Align-Existing: 1 25 | -------------------------------------------------------------------------------- /tests/external/mcsc/mcsc_output_20130726/original_cam_centers.dat: -------------------------------------------------------------------------------- 1 | 0.38877566795033641 -0.25224323017973899 0.54269896014519803 2 | 0.43992018849672399 0.19838405533526099 0.5530006979363804 3 | -0.36633996302366301 0.17534422404396213 0.49834434355408341 4 | -0.35485027557235815 -0.16901931968931816 0.50765063358276696 5 | -------------------------------------------------------------------------------- /tests/external/mcsc/test_mcsc.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | from pymvg.camera_model import CameraModel 4 | from pymvg.multi_camera_system import MultiCameraSystem 5 | import os 6 | import tempfile 7 | import shutil 8 | from collections import defaultdict 9 | import numpy as np 10 | import multicamselfcal.execute as mcsce 11 | import pytest 12 | 13 | def make_default_system(with_separate_distorions=False): 14 | '''helper function to generate an instance of MultiCameraSystem''' 15 | if with_separate_distorions: 16 | raise NotImplementedError 17 | camxs = np.linspace(-2,2,3) 18 | camzs = np.linspace(-2,2,3).tolist() 19 | camzs.pop(1) 20 | cameras = [] 21 | lookat = np.array( (0.0,0,0) ) 22 | up = np.array( (0.0,0,1) ) 23 | 24 | for enum,camx in enumerate(camxs): 25 | center = np.array( (camx, -5, 0) ) 26 | name = 'camx_%d'%(enum+1,) 27 | cam = CameraModel.load_camera_simple(name=name, 28 | fov_x_degrees=45, 29 | eye=center, 30 | lookat=lookat, 31 | up=up, 32 | ) 33 | cameras.append(cam) 34 | 35 | 36 | for enum,camz in enumerate(camzs): 37 | center = np.array( (0, -5, camz) ) 38 | name = 'camz_%d'%(enum+1,) 39 | cam = CameraModel.load_camera_simple(name=name, 40 | fov_x_degrees=45, 41 | eye=center, 42 | lookat=lookat, 43 | up=up, 44 | ) 45 | cameras.append(cam) 46 | 47 | system = MultiCameraSystem(cameras) 48 | return system 49 | 50 | def get_default_points(): 51 | N = 500 52 | pts_3d = 1.0*np.random.randn(N,3) 53 | return pts_3d 54 | 55 | def is_close(sys1,sys2,pts_3d): 56 | names = sys1.get_names() 57 | names2 = sys2.get_names() 58 | assert names==names2 59 | 60 | for pt_3d in pts_3d: 61 | for name in names: 62 | c1 = sys1.find2d( name, pt_3d ) 63 | c2 = sys2.find2d( name, pt_3d ) 64 | print('%s: %s %s' % (name, c1, c2)) 65 | if not np.allclose(c1,c2): 66 | return False 67 | return True 68 | 69 | # We know this test will fail and it has been failing for a long time. TODO: 70 | # make this test pass. 71 | @pytest.mark.xfail 72 | def test_mcsc_roundtrip(with_rad_files=False,align_existing=False): 73 | np.random.seed(3) 74 | mcscdir = os.path.join( os.path.dirname(mcsce.__file__), 75 | '..', '..', 'MultiCamSelfCal' ) 76 | mcscdir = os.path.abspath(mcscdir) 77 | out_dirname = tempfile.mkdtemp() 78 | try: 79 | mcsc = mcsce.MultiCamSelfCal(out_dirname=out_dirname,mcscdir=mcscdir) 80 | if with_rad_files: 81 | orig_cams = make_default_system(with_separate_distorions=True) 82 | else: 83 | orig_cams = make_default_system() 84 | 85 | cam_resolutions = dict( (n['name'], (n['width'],n['height'])) 86 | for n in orig_cams.to_dict()['camera_system']) 87 | 88 | pts_3d = get_default_points() 89 | 90 | if 0: 91 | import matplotlib.pyplot as plt 92 | from mpl_toolkits.mplot3d import Axes3D 93 | 94 | from pymvg.plot_utils import plot_camera 95 | np.set_printoptions(precision=3, suppress=True) 96 | fig = plt.figure() 97 | ax = fig.add_subplot(111, projection='3d') 98 | for name,cam in orig_cams.get_camera_dict().iteritems(): 99 | print() 100 | print(name,'-'*80) 101 | print(' center:',cam.get_camcenter()) 102 | print(' lookat:',cam.get_lookat()) 103 | print(' up:',cam.get_up()) 104 | print(' P') 105 | print(cam.P) 106 | print() 107 | plot_camera( ax, cam)#, scale = dim/5.0 ) 108 | ax.plot( pts_3d[:,0], pts_3d[:,1], pts_3d[:,2], 'k.') 109 | plt.show() 110 | 111 | cam_points=defaultdict(list) 112 | 113 | nan_tup = (np.nan, np.nan) 114 | for pt_3d in pts_3d: 115 | valid = True 116 | for name in orig_cams.get_names(): 117 | w,h = cam_resolutions[name] 118 | 119 | x,y = orig_cams.find2d( name, pt_3d ) 120 | 121 | if 1: 122 | # add noise to the 2D images 123 | xn, yn = 0.01*np.random.randn( 2 ) 124 | 125 | x += xn 126 | y += yn 127 | 128 | if (0 <= x) and (x < w) and (0 <= y) and (y < h): 129 | cam_points[name].append((x,y)) 130 | else: 131 | cam_points[name].append(nan_tup) 132 | valid = False 133 | if not valid: 134 | for name in orig_cams.get_names(): 135 | cam_points[name].pop() 136 | 137 | cam_ids=orig_cams.get_names() 138 | if with_rad_files: 139 | cam_calibrations = {} # dictionary of .yaml filenames with ROS yaml format 140 | raise NotImplementedError 141 | else: 142 | cam_calibrations = {} 143 | 144 | if align_existing: 145 | cam_centers = np.array([ orig_cams.get_cam(n).get_camcenter() for n in orig_cams.get_names() ]).T 146 | else: 147 | cam_centers = [] 148 | 149 | mcsc.create_from_cams(cam_ids=cam_ids, 150 | cam_resolutions=cam_resolutions, 151 | cam_points=cam_points, 152 | cam_calibrations=cam_calibrations, 153 | ) 154 | 155 | result = mcsc.execute() 156 | raw_cams = MultiCameraSystem.from_mcsc( result ) 157 | if align_existing: 158 | aligned_cams = raw_cams 159 | else: 160 | aligned_cams = raw_cams.get_aligned_copy( orig_cams ) 161 | 162 | assert is_close(orig_cams,aligned_cams,pts_3d) 163 | finally: 164 | shutil.rmtree(out_dirname) 165 | 166 | -------------------------------------------------------------------------------- /tests/external/opencv/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/pymvg/1e7ad0bdd5242146b3c916ea6dcbce101261928b/tests/external/opencv/__init__.py -------------------------------------------------------------------------------- /tests/external/opencv/test_versus_opencv.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | import os 4 | import numpy as np 5 | 6 | import cv2 as cv # ubuntu: sudo apt-get install python3-opencv 7 | from unittest import SkipTest 8 | 9 | from pymvg.ros_compat import sensor_msgs, geometry_msgs 10 | 11 | from tests.utils import _build_test_camera, _build_points_3d, get_default_options 12 | from pymvg.extern.opencv.npcv import numpy2opencv_image, numpy2opencv_pointmat,\ 13 | opencv_pointmat2numpy 14 | 15 | def noop(*args,**kwargs): 16 | pass 17 | 18 | DEBUG=int(os.environ.get('DEBUG',0)) 19 | if DEBUG: 20 | debug = print 21 | else: 22 | debug = noop 23 | 24 | def pytest_generate_tests(metafunc): 25 | if "cam_opts" in metafunc.fixturenames: 26 | all_options = get_default_options() 27 | metafunc.parametrize("cam_opts", all_options) 28 | if "distorted" in metafunc.fixturenames: 29 | metafunc.parametrize("distorted", [True, False]) 30 | 31 | def test_undistortion(cam_opts): 32 | cam = _build_test_camera(**cam_opts) 33 | 34 | step = 5 35 | border = 65 36 | 37 | distorteds = [] 38 | for row in range(border, cam.height-border, step): 39 | for col in range(border, cam.width-border, step): 40 | distorted = [col, row] 41 | distorteds.append(distorted) 42 | npdistorted = np.array(distorteds,dtype=float) 43 | 44 | src = numpy2opencv_pointmat(npdistorted) 45 | dst = cv.CloneMat(src) 46 | cv.UndistortPoints(src, dst, 47 | numpy2opencv_image(cam.get_K()), 48 | numpy2opencv_image(cam.get_D()), 49 | R = numpy2opencv_image(cam.get_rect()), 50 | P = numpy2opencv_image(cam.get_P())) 51 | undistorted_cv = opencv_pointmat2numpy(dst) 52 | 53 | undistorted_np = cam.undistort( npdistorted ) 54 | assert undistorted_cv.shape == undistorted_np.shape 55 | if cam.is_opencv_compatible(): 56 | assert np.allclose(undistorted_cv, undistorted_np) 57 | else: 58 | raise SkipTest("Test %s is skipped: %s" %( 59 | check_undistortion.__name__, 60 | 'camera model is not OpenCV compatible, skipping test')) 61 | 62 | def test_projection(cam_opts,distorted): 63 | cam = _build_test_camera(**cam_opts) 64 | R = cam.get_rect() 65 | if not np.allclose(R, np.eye(3)): 66 | # opencv's ProjectPoints2 does not take into account 67 | # rectifciation matrix, thus we skip this test. 68 | raise SkipTest("Test %s is skipped: %s" %( 69 | check_projection.__name__, 70 | 'cannot check if rectification matrix is not unity')) 71 | 72 | pts3D = _build_points_3d() 73 | n_pts = len(pts3D) 74 | 75 | src = numpy2opencv_image(pts3D) 76 | dst = numpy2opencv_pointmat(np.empty( (n_pts, 2) )) 77 | 78 | t = np.array(cam.get_translation(),copy=True) 79 | t.shape = 3,1 80 | 81 | R = cam.get_rotation() 82 | rvec = numpy2opencv_image(np.empty( (1,3) )) 83 | cv.Rodrigues2(numpy2opencv_image(R), rvec) 84 | 85 | if distorted: 86 | K = cam.get_K() 87 | cv_distortion = numpy2opencv_image(cam.get_D()) 88 | else: 89 | K = cam.get_P()[:3,:3] 90 | cv_distortion = numpy2opencv_image(np.zeros((5,1))) 91 | 92 | cv.ProjectPoints2(src, 93 | rvec, 94 | numpy2opencv_image(t), 95 | numpy2opencv_image(K), 96 | cv_distortion, 97 | dst) 98 | result_cv = opencv_pointmat2numpy(dst) 99 | 100 | result_np = cam.project_3d_to_pixel( pts3D, distorted=distorted ) 101 | assert result_cv.shape == result_np.shape 102 | if cam.is_opencv_compatible(): 103 | try: 104 | assert np.allclose(result_cv, result_np) 105 | except: 106 | debug() 107 | debug('result_cv') 108 | debug(result_cv) 109 | debug('result_np') 110 | debug(result_np) 111 | debug() 112 | raise 113 | else: 114 | raise SkipTest("Test %s is skipped: %s" %( 115 | check_projection.__name__, 116 | 'camera model is not OpenCV compatible, skipping test')) 117 | -------------------------------------------------------------------------------- /tests/external/ros/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/strawlab/pymvg/1e7ad0bdd5242146b3c916ea6dcbce101261928b/tests/external/ros/__init__.py -------------------------------------------------------------------------------- /tests/external/ros/test_full_ros_pipeline.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from __future__ import print_function 3 | import numpy as np 4 | from tests.utils import make_M, _build_test_camera, get_default_options 5 | import tests.fill_polygon as fill_polygon 6 | import tarfile, time, os, tempfile 7 | import subprocess 8 | import cv2 as cv # ubuntu: sudo apt-get install python3-opencv 9 | 10 | from io import StringIO 11 | 12 | DRAW=int(os.environ.get('DRAW','0')) 13 | if DRAW: 14 | import matplotlib.pyplot as plt 15 | 16 | D2R = np.pi/180.0 17 | R2D = 1/D2R 18 | 19 | from pymvg.util import get_rotation_matrix_and_quaternion 20 | from pymvg.camera_model import CameraModel 21 | 22 | try: 23 | import roslib 24 | except ModuleNotFoundError: 25 | have_ros = False 26 | else: 27 | have_ros = True 28 | 29 | if have_ros: 30 | roslib.load_manifest('camera_calibration') 31 | roslib.load_manifest('rosbag') 32 | roslib.load_manifest('tf') 33 | import camera_calibration.calibrator 34 | import tf.transformations 35 | import rosbag 36 | else: 37 | from unittest import SkipTest 38 | 39 | def pytest_generate_tests(metafunc): 40 | if "cam_opts" in metafunc.fixturenames: 41 | all_options = get_default_options() 42 | metafunc.parametrize("cam_opts", all_options) 43 | 44 | def get_np_array_as_png_buf(im): 45 | import scipy.misc 46 | output = StringIO() 47 | pil_im = scipy.misc.toimage( im ) 48 | pil_im.save( output, format='PNG') 49 | return output.getvalue() 50 | 51 | def png_buf_to_opencv(filedata): 52 | imagefiledata = cv.CreateMat(1, len(filedata), cv.CV_8UC1) 53 | cv.SetData(imagefiledata, filedata, len(filedata)) 54 | return cv.DecodeImageM(imagefiledata) 55 | 56 | def np_image_to_opencv(im): 57 | return png_buf_to_opencv(get_np_array_as_png_buf(im)) 58 | 59 | def draw_checkerboard(check_pixels,cw,ch,imw,imh): 60 | assert len(check_pixels)==(cw*ch) 61 | x = check_pixels[:,0] 62 | y = check_pixels[:,1] 63 | assert np.all( (0<=x) & (x0) and (i0) and (j 2: 219 | raise ValueError('checkboard corner localization failed') 220 | if DRAW: 221 | f = plt.figure() 222 | ax = f.add_subplot(111) 223 | ax.plot(cnp[:,0],cnp[:,1],'r+',label='cv') 224 | ax.plot(myp[:,0],myp[:,1],'bx',label='truth') 225 | ax.legend() 226 | goodcorners.append( (corners,board) ) 227 | perfectcorners.append( ([(x,y) for x,y in imd['pix']], board) ) 228 | 229 | cal.cal_fromcorners(goodcorners) 230 | msg = cal.as_message() 231 | 232 | perfectcal.cal_fromcorners(perfectcorners) 233 | msg2 = perfectcal.as_message() 234 | return {'good':msg, 'perfect':msg2} 235 | 236 | def calc_mean_reproj_error(self,msg): 237 | ros_cam = CameraModel._from_parts(intrinsics=msg) 238 | all_ims = [] 239 | for imd in self.db: 240 | ros_pix = ros_cam.project_3d_to_pixel(imd['cc'], distorted=True) 241 | d = (ros_pix-imd['pix'])**2 242 | drows = np.sqrt(np.sum(d, axis=1)) 243 | mean_d = np.mean(drows) 244 | all_ims.append(mean_d) 245 | mean_err = np.mean(all_ims) 246 | return mean_err 247 | 248 | @pytest.mark.parametrize("use_distortion", [True, False]) 249 | def test_ros_pipeline(use_distortion): 250 | pm = ROSPipelineMimic(use_distortion=use_distortion) 251 | pm.generate_camera() 252 | pm.generate_images() 253 | #pm.save_tarball('/tmp/pipeline-mimic.tar.gz') # optional 254 | cals = pm.run_ros_calibrator() 255 | print(cals) 256 | err1 = pm.calc_mean_reproj_error(cals['perfect']) 257 | err2 = pm.calc_mean_reproj_error(cals['good']) 258 | print(err1,err2) 259 | 260 | if DRAW: 261 | from mpl_toolkits.mplot3d import Axes3D 262 | from pymvg.plot_utils import plot_camera 263 | 264 | f = plt.figure() 265 | ax = f.add_subplot(111,projection='3d') 266 | for imd in pm.db: 267 | wcs3d = imd['wc'] 268 | 269 | ax.plot(wcs3d[:,0],wcs3d[:,1],wcs3d[:,2], 'o-') 270 | 271 | plot_camera( ax, pm.cam )#, scale=10, axes_size=5.0 ) 272 | 273 | if DRAW: 274 | print('using perfect point data, mean reprojection error is %s'%err1) 275 | print('mean reprojection error is %s'%err2) 276 | plt.show() 277 | 278 | assert err1 < 1.0 279 | 280 | # FIXME: why is the error we get so large? Logically, it must be 281 | # from detect checkerboard corners code, so it's not hugely 282 | # important. Nevertheless, this is an annoyingly large error. 283 | assert err2 < 30.0 284 | 285 | def test_roundtrip_ros_tf(cam_opts): 286 | 287 | cam1 = _build_test_camera(**cam_opts) 288 | translation, rotation = cam1.get_ROS_tf() 289 | i = cam1.get_intrinsics_as_bunch() 290 | cam2 = CameraModel.load_camera_from_ROS_tf( translation=translation, 291 | rotation=rotation, 292 | intrinsics = i, 293 | name = cam1.name) 294 | assert cam1==cam2 295 | 296 | def test_bagfile_roundtrip(cam_opts): 297 | """check that roundtrip of camera model to/from a bagfile works""" 298 | 299 | cam = _build_test_camera(**cam_opts) 300 | fname = tempfile.mktemp(suffix='.bag') 301 | try: 302 | with open(fname,mode='wb') as fd: 303 | cam.save_to_bagfile(fd, roslib) 304 | 305 | with open(fname,mode='r') as fd: 306 | bag = rosbag.Bag(fd, 'r') 307 | cam2 = CameraModel.load_camera_from_opened_bagfile( bag ) 308 | finally: 309 | os.unlink(fname) 310 | 311 | verts = np.array([[ 0.042306, 0.015338, 0.036328], 312 | [ 0.03323, 0.030344, 0.041542], 313 | [ 0.03323, 0.030344, 0.041542], 314 | [ 0.03323, 0.030344, 0.041542], 315 | [ 0.036396, 0.026464, 0.052408]]) 316 | 317 | expected = cam.project_3d_to_pixel(verts) 318 | actual = cam2.project_3d_to_pixel(verts) 319 | assert np.allclose( expected, actual ) 320 | 321 | if __name__=='__main__': 322 | test_ros_pipeline() 323 | -------------------------------------------------------------------------------- /tests/fill_polygon.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | def posint(x,maxval=np.inf): 4 | x = int(x) 5 | if x < 0: 6 | x = 0 7 | if x>maxval: 8 | return maxval 9 | return x 10 | 11 | def fill_polygon(pts,image,fill_value=255): 12 | if len(pts)>=3: 13 | height, width = image.shape[:2] 14 | pts = [ (posint(y,height-1),posint(x,width-1)) for (x,y) in pts] 15 | if image.ndim == 3: 16 | _fill_polygon(pts, image[:,:,0], color=fill_value) 17 | else: 18 | _fill_polygon(pts, image[:,:], color=fill_value) 19 | 20 | # from https://raw.github.com/luispedro/mahotas/master/mahotas/polygon.py 21 | def _fill_polygon(polygon, canvas, color=1): 22 | ''' 23 | fill_polygon([(y0,x0), (y1,x1),...], canvas, color=1) 24 | 25 | Draw a filled polygon in canvas 26 | 27 | Parameters 28 | ---------- 29 | polygon : list of pairs 30 | a list of (y,x) points 31 | canvas : ndarray 32 | where to draw, will be modified in place 33 | color : integer, optional 34 | which colour to use (default: 1) 35 | ''' 36 | # algorithm adapted from: http://www.alienryderflex.com/polygon_fill/ 37 | if not polygon: 38 | return 39 | min_y = min(y for y,x in polygon) 40 | max_y = max(y for y,x in polygon) 41 | polygon = [(float(y),float(x)) for y,x in polygon] 42 | for y in xrange(min_y, max_y+1): 43 | nodes = [] 44 | j = -1 45 | for i,p in enumerate(polygon): 46 | pj = polygon[j] 47 | if p[0] < y and pj[0] >= y or pj[0] < y and p[0] >= y: 48 | dy = pj[0] - p[0] 49 | if dy: 50 | nodes.append( (p[1] + (y-p[0])/(pj[0]-p[0])*(pj[1]-p[1])) ) 51 | elif p[0] == y: 52 | nodes.append(p[1]) 53 | j = i 54 | nodes.sort() 55 | for n,nn in zip(nodes[::2],nodes[1::2]): 56 | nn += 1 57 | canvas[y,n:nn] = color 58 | 59 | def test_fill(): 60 | poly = [ [1,1], 61 | [1, 3], 62 | [3, 2], 63 | ] 64 | shape = (5,5,3) 65 | results = [] 66 | for dtype in [float, int, np.uint8]: 67 | image = np.zeros( shape, dtype=dtype ) 68 | fill_polygon( poly, image, fill_value=255 ) 69 | results.append( image ) 70 | 71 | for i in range(len(results)-1): 72 | i1 = results[i] 73 | i2 = results[i+1] 74 | assert np.allclose(i1,i2) 75 | -------------------------------------------------------------------------------- /tests/pymvg_camsystem_example.json: -------------------------------------------------------------------------------- 1 | { "__pymvg_file_version__": "1.0", 2 | "camera_system": [ 3 | {"name": "cam1", 4 | "width": 640, 5 | "height": 480, 6 | "P": [[ 320.0, 0, 319.99999999999994, 0 ], 7 | [ 0, 320.00000000000006, 240.0, 0 ], 8 | [ 0, 0, 1.0, 0 ]], 9 | "K": [[ 320.0, 0, 319.99999999999994 ], 10 | [ 0, 320.00000000000006, 240.0 ], 11 | [ 0, 0, 1.0 ]], 12 | "D": [ 0.2, 0.3, 0.1, 0.1, 0.1 ], 13 | "R": [[ 1.0, 0, 0 ], 14 | [ 0, 1.0, 0 ], 15 | [ 0, 0, 1.0 ]], 16 | "Q": [[ -1.0000000000000004, 0, 0 ], 17 | [ 0, 1.0, 0 ], 18 | [ 0, 0, -1.0000000000000004 ]], 19 | "translation": [ 0, 0, 0.9000000000000005 ] 20 | }, 21 | {"name": "cam2", 22 | "width": 640, 23 | "height": 480, 24 | "P": [[ 320.0, 0, 319.99999999999994, 0 ], 25 | [ 0, 320.00000000000006, 240.0, 0 ], 26 | [ 0, 0, 1.0, 0 ]], 27 | "K": [[ 320.0, 0, 319.99999999999994 ], 28 | [ 0, 320.00000000000006, 240.0 ], 29 | [ 0, 0, 1.0 ]], 30 | "D": [ 0, 0, 0, 0, 0 ], 31 | "R": [[ 1.0, 0, 0 ], 32 | [ 0, 1.0, 0 ], 33 | [ 0, 0, 1.0 ]], 34 | "Q": [[ 0, 0, 0.9999999999999999 ], 35 | [ 0.847998304005088, 0.5299989400031799, 0 ], 36 | [ -0.5299989400031798, 0.847998304005088, 0 ]], 37 | "translation": [ 0, 0, 0.9433981132056602 ] 38 | }, 39 | {"name": "cam3", 40 | "width": 640, 41 | "height": 480, 42 | "P": [[ 320.0, 0, 319.99999999999994, 0 ], 43 | [ 0, 320.00000000000006, 240.0, 0 ], 44 | [ 0, 0, 1.0, 0 ]], 45 | "K": [[ 320.0, 0, 319.99999999999994 ], 46 | [ 0, 320.00000000000006, 240.0 ], 47 | [ 0, 0, 1.0 ]], 48 | "D": [ 0, 0, 0, 0, 0 ], 49 | "R": [[ 1.0, 0, 0 ], 50 | [ 0, 1.0, 0 ], 51 | [ 0, 0, 1.0 ]], 52 | "Q": [[ 0, 0, 1.0000000000000002 ], 53 | [ -0.7071067811865475, 0.7071067811865477, 0 ], 54 | [ -0.7071067811865478, -0.7071067811865475, 0 ]], 55 | "translation": [ 0, 0, 0.7071067811865475 ] 56 | } 57 | ] 58 | } -------------------------------------------------------------------------------- /tests/roscal.yaml: -------------------------------------------------------------------------------- 1 | image_width: 659 2 | image_height: 494 3 | camera_name: Basler_21029382 4 | camera_matrix: 5 | rows: 3 6 | cols: 3 7 | data: [516.385667640757, 0, 339.167079537312, 0, 516.125799367807, 227.37993524141, 0, 0, 1] 8 | distortion_model: plumb_bob 9 | distortion_coefficients: 10 | rows: 1 11 | cols: 5 12 | data: [-0.331416226762003, 0.143584747015566, 0.00314558656668844, -0.00393597842852019, 0] 13 | rectification_matrix: 14 | rows: 3 15 | cols: 3 16 | data: [1, 0, 0, 0, 1, 0, 0, 0, 1] 17 | projection_matrix: 18 | rows: 3 19 | cols: 4 20 | data: [444.369750976562, 0, 337.107817516087, 0, 0, 474.186859130859, 225.062742824321, 0, 0, 0, 1, 0] -------------------------------------------------------------------------------- /tests/skew_pixels.json: -------------------------------------------------------------------------------- 1 | { "__pymvg_file_version__": "1.0", 2 | "camera_system": [ 3 | {"name": "Basler_21425994", 4 | "width": 659, 5 | "height": 494, 6 | "P": [[ -10890.946456541287, -204.29132593725262, -6097.739543773519, 0 ], 7 | [ 0, -10828.089766638526, -1777.60375276579, 0 ], 8 | [ 0, 0, -14.619882925258498, 0 ]], 9 | "K": [[ 567.913443, 0, 334.898725 ], 10 | [ 0, 570.127226, 262.289768 ], 11 | [ 0, 0, 1.0 ]], 12 | "D": [ -0.336611, 0.134028, 0.000438, -0.003011, 0 ], 13 | "R": [[ -19.177123892348657, 0, 0 ], 14 | [ 0, -18.99240954095135, 0 ], 15 | [ -11.326793454286163, -3.577961949085712, 1.0 ]], 16 | "Q": [[ -0.09389425805717533, -0.8803682536532833, -0.4649038677655421 ], 17 | [ 0.9093245388963607, -0.2659620591947953, 0.31998916548812273 ], 18 | [ -0.4053550928072985, -0.39270334990719424, 0.8255127665317201 ]], 19 | "translation": [ 0.035531225112425534, -0.06642969265767877, -0.5072144584132425 ] 20 | }, 21 | {"name": "Basler_21425998", 22 | "width": 659, 23 | "height": 494, 24 | "P": [[ -10296.618183221342, 94.15696548941537, -3930.8467631449157, 0 ], 25 | [ 0, -10275.999622969963, -2503.1593334806585, 0 ], 26 | [ 0, 0, -14.61987707508367, 0 ]], 27 | "K": [[ 529.006476, 0, 320.079021 ], 28 | [ 0, 531.379059, 241.539826 ], 29 | [ 0, 0, 1.0 ]], 30 | "D": [ -0.326318, 0.109987, -0.000294, 0.000452, 0 ], 31 | "R": [[ -19.46406830607749, 0, 0 ], 32 | [ 0, -19.33836015726386, 0 ], 33 | [ -8.035678157075937, -5.165237720594215, 1.0 ]], 34 | "Q": [[ 0.986408117134965, 0.156282500394611, -0.050742551381079186 ], 35 | [ -0.11164726040006927, 0.8640574130479857, 0.4908560646482652 ], 36 | [ 0.12055669079487946, -0.47851913966656673, 0.8697617589187034 ]], 37 | "translation": [ -0.04249306337067645, -0.02423841249614171, -0.45190934000805755 ] 38 | }, 39 | {"name": "Basler_21426001", 40 | "width": 659, 41 | "height": 494, 42 | "P": [[ -10239.099497642479, -125.54084703272292, -5683.168875262537, 0 ], 43 | [ 0, -10113.071699733793, -1531.7176640397865, 0 ], 44 | [ 0, 0, -14.619880513239497, 0 ]], 45 | "K": [[ 554.542594, 0, 336.163566 ], 46 | [ 0, 557.419006, 242.383574 ], 47 | [ 0, 0, 1.0 ]], 48 | "D": [ -0.319159, 0.11219, -0.001344, -0.000155, 0 ], 49 | "R": [[ -18.46404515798561, 0, 0 ], 50 | [ 0, -18.14267470408749, 0 ], 51 | [ -10.854589902362914, -3.1827067590870532, 1.0 ]], 52 | "Q": [[ -0.8887023079872123, -0.4573131788816193, -0.03275460577371585 ], 53 | [ 0.3667438701886832, -0.7519340969591706, 0.5478084040147834 ], 54 | [ -0.2751493075717796, 0.47482604209477175, 0.8359623725332286 ]], 55 | "translation": [ 0.026033378203590092, -0.04931639578273312, -0.4245506654024399 ] 56 | }, 57 | {"name": "Basler_21426006", 58 | "width": 659, 59 | "height": 494, 60 | "P": [[ -10858.321835347268, -51.90033220223995, -5701.519499967621, 0 ], 61 | [ 0, -10881.490829318649, -2782.437621239361, 0 ], 62 | [ 0, 0, -14.619881381976395, 0 ]], 63 | "K": [[ 554.131398, 0, 321.346562 ], 64 | [ 0, 555.660776, 242.352488 ], 65 | [ 0, 0, 1.0 ]], 66 | "D": [ -0.325807, 0.11428, 0.000612, -0.000316, 0 ], 67 | "R": [[ -19.595211306447695, 0, 0 ], 68 | [ 0, -19.582974540061198, 0 ], 69 | [ -10.86902146982767, -5.4435911978774625, 1.0 ]], 70 | "Q": [[ -0.7068596946064027, 0.7057571987412011, -0.04749893226080523 ], 71 | [ -0.6430455136243106, -0.6131706131053289, 0.4588183372879535 ], 72 | [ 0.2946893950392352, 0.3548641650674359, 0.8872595926798432 ]], 73 | "translation": [ 0.03626446430051513, -0.004780633307382862, -0.4486097273050089 ] 74 | } 75 | ] 76 | } -------------------------------------------------------------------------------- /tests/skew_pixels_no_distortion.json: -------------------------------------------------------------------------------- 1 | { "__pymvg_file_version__": "1.0", 2 | "camera_system": [ 3 | {"name": "Basler_21425994", 4 | "width": 659, 5 | "height": 494, 6 | "P": [[ -10890.946456541287, -204.29132593725262, -6097.739543773519, 0 ], 7 | [ 0, -10828.089766638526, -1777.60375276579, 0 ], 8 | [ 0, 0, -14.619882925258498, 0 ]], 9 | "K": [[ 567.913443, 0, 334.898725 ], 10 | [ 0, 570.127226, 262.289768 ], 11 | [ 0, 0, 1.0 ]], 12 | "D": [ 0, 0, 0, 0, 0], 13 | "R": [[ -19.177123892348657, 0, 0 ], 14 | [ 0, -18.99240954095135, 0 ], 15 | [ -11.326793454286163, -3.577961949085712, 1.0 ]], 16 | "Q": [[ -0.09389425805717533, -0.8803682536532833, -0.4649038677655421 ], 17 | [ 0.9093245388963607, -0.2659620591947953, 0.31998916548812273 ], 18 | [ -0.4053550928072985, -0.39270334990719424, 0.8255127665317201 ]], 19 | "translation": [ 0.035531225112425534, -0.06642969265767877, -0.5072144584132425 ] 20 | }, 21 | {"name": "Basler_21425998", 22 | "width": 659, 23 | "height": 494, 24 | "P": [[ -10296.618183221342, 94.15696548941537, -3930.8467631449157, 0 ], 25 | [ 0, -10275.999622969963, -2503.1593334806585, 0 ], 26 | [ 0, 0, -14.61987707508367, 0 ]], 27 | "K": [[ 529.006476, 0, 320.079021 ], 28 | [ 0, 531.379059, 241.539826 ], 29 | [ 0, 0, 1.0 ]], 30 | "D": [ 0, 0, 0, 0, 0], 31 | "R": [[ -19.46406830607749, 0, 0 ], 32 | [ 0, -19.33836015726386, 0 ], 33 | [ -8.035678157075937, -5.165237720594215, 1.0 ]], 34 | "Q": [[ 0.986408117134965, 0.156282500394611, -0.050742551381079186 ], 35 | [ -0.11164726040006927, 0.8640574130479857, 0.4908560646482652 ], 36 | [ 0.12055669079487946, -0.47851913966656673, 0.8697617589187034 ]], 37 | "translation": [ -0.04249306337067645, -0.02423841249614171, -0.45190934000805755 ] 38 | }, 39 | {"name": "Basler_21426001", 40 | "width": 659, 41 | "height": 494, 42 | "P": [[ -10239.099497642479, -125.54084703272292, -5683.168875262537, 0 ], 43 | [ 0, -10113.071699733793, -1531.7176640397865, 0 ], 44 | [ 0, 0, -14.619880513239497, 0 ]], 45 | "K": [[ 554.542594, 0, 336.163566 ], 46 | [ 0, 557.419006, 242.383574 ], 47 | [ 0, 0, 1.0 ]], 48 | "D": [ 0, 0, 0, 0, 0], 49 | "R": [[ -18.46404515798561, 0, 0 ], 50 | [ 0, -18.14267470408749, 0 ], 51 | [ -10.854589902362914, -3.1827067590870532, 1.0 ]], 52 | "Q": [[ -0.8887023079872123, -0.4573131788816193, -0.03275460577371585 ], 53 | [ 0.3667438701886832, -0.7519340969591706, 0.5478084040147834 ], 54 | [ -0.2751493075717796, 0.47482604209477175, 0.8359623725332286 ]], 55 | "translation": [ 0.026033378203590092, -0.04931639578273312, -0.4245506654024399 ] 56 | }, 57 | {"name": "Basler_21426006", 58 | "width": 659, 59 | "height": 494, 60 | "P": [[ -10858.321835347268, -51.90033220223995, -5701.519499967621, 0 ], 61 | [ 0, -10881.490829318649, -2782.437621239361, 0 ], 62 | [ 0, 0, -14.619881381976395, 0 ]], 63 | "K": [[ 554.131398, 0, 321.346562 ], 64 | [ 0, 555.660776, 242.352488 ], 65 | [ 0, 0, 1.0 ]], 66 | "D": [ 0, 0, 0, 0, 0], 67 | "R": [[ -19.595211306447695, 0, 0 ], 68 | [ 0, -19.582974540061198, 0 ], 69 | [ -10.86902146982767, -5.4435911978774625, 1.0 ]], 70 | "Q": [[ -0.7068596946064027, 0.7057571987412011, -0.04749893226080523 ], 71 | [ -0.6430455136243106, -0.6131706131053289, 0.4588183372879535 ], 72 | [ 0.2946893950392352, 0.3548641650674359, 0.8872595926798432 ]], 73 | "translation": [ 0.03626446430051513, -0.004780633307382862, -0.4486097273050089 ] 74 | } 75 | ] 76 | } -------------------------------------------------------------------------------- /tests/synthetic.json: -------------------------------------------------------------------------------- 1 | { "__pymvg_file_version__": "1.0", 2 | "camera_system": [ 3 | {"name": "theta: 0", 4 | "width": 640, 5 | "height": 480, 6 | "P": [[ 300.0, 0, 320.0, 0 ], 7 | [ 0, 300.0, 240.0, 0 ], 8 | [ 0, 0, 1.0, 0 ]], 9 | "K": [[ 300.0, 0, 320.0 ], 10 | [ 0, 300.0, 240.0 ], 11 | [ 0, 0, 1.0 ]], 12 | "D": [ 0, 0, 0, 0, 0 ], 13 | "R": [[ 1.0, 0, 0 ], 14 | [ 0, 1.0, 0 ], 15 | [ 0, 0, 1.0 ]], 16 | "Q": [[ 1.0, 0, 0 ], 17 | [ 0, 0, -1.0 ], 18 | [ 0, 1.0, 0 ]], 19 | "translation": [ 0, 5.0, 0 ] 20 | }, 21 | {"name": "theta: 72", 22 | "width": 640, 23 | "height": 480, 24 | "P": [[ 300.0, 0, 320.0, 0 ], 25 | [ 0, 300.0, 240.0, 0 ], 26 | [ 0, 0, 1.0, 0 ]], 27 | "K": [[ 300.0, 0, 320.0 ], 28 | [ 0, 300.0, 240.0 ], 29 | [ 0, 0, 1.0 ]], 30 | "D": [ 0, 0, 0, 0, 0 ], 31 | "R": [[ 1.0, 0, 0 ], 32 | [ 0, 1.0, 0 ], 33 | [ 0, 0, 1.0 ]], 34 | "Q": [[ 0.30901699437494706, 0, 0.9510565162951538 ], 35 | [ 0.9510565162951538, 0, -0.3090169943749473 ], 36 | [ 0, 1.0000000000000002, 0 ]], 37 | "translation": [ -5.496923367975643, -0.7374506672336326, 0 ] 38 | }, 39 | {"name": "theta: 144", 40 | "width": 640, 41 | "height": 480, 42 | "P": [[ 300.0, 0, 320.0, 0 ], 43 | [ 0, 300.0, 240.0, 0 ], 44 | [ 0, 0, 1.0, 0 ]], 45 | "K": [[ 300.0, 0, 320.0 ], 46 | [ 0, 300.0, 240.0 ], 47 | [ 0, 0, 1.0 ]], 48 | "D": [ 0, 0, 0, 0, 0 ], 49 | "R": [[ 1.0, 0, 0 ], 50 | [ 0, 1.0, 0 ], 51 | [ 0, 0, 1.0 ]], 52 | "Q": [[ -0.8090169943749475, 0, 0.5877852522924731 ], 53 | [ 0.5877852522924731, 0, 0.8090169943749475 ], 54 | [ 0, 1.0, 0 ]], 55 | "translation": [ 0.944355311537382, -6.866454182878608, 2.4646951146678474e-15 ] 56 | }, 57 | {"name": "theta: 216", 58 | "width": 640, 59 | "height": 480, 60 | "P": [[ 300.0, 0, 320.0, 0 ], 61 | [ 0, 300.0, 240.0, 0 ], 62 | [ 0, 0, 1.0, 0 ]], 63 | "K": [[ 300.0, 0, 320.0 ], 64 | [ 0, 300.0, 240.0 ], 65 | [ 0, 0, 1.0 ]], 66 | "D": [ 0, 0, 0, 0, 0 ], 67 | "R": [[ 1.0, 0, 0 ], 68 | [ 0, 1.0, 0 ], 69 | [ 0, 0, 1.0 ]], 70 | "Q": [[ -0.8090169943749472, 0, -0.5877852522924731 ], 71 | [ -0.5877852522924731, 0, 0.8090169943749473 ], 72 | [ 0, 0.9999999999999999, 0 ]], 73 | "translation": [ 8.763848620961985, 0.18696884463107, 1.6209256159527287e-15 ] 74 | }, 75 | {"name": "theta: 288", 76 | "width": 640, 77 | "height": 480, 78 | "P": [[ 300.0, 0, 320.0, 0 ], 79 | [ 0, 300.0, 240.0, 0 ], 80 | [ 0, 0, 1.0, 0 ]], 81 | "K": [[ 300.0, 0, 320.0 ], 82 | [ 0, 300.0, 240.0 ], 83 | [ 0, 0, 1.0 ]], 84 | "D": [ 0, 0, 0, 0, 0 ], 85 | "R": [[ 1.0, 0, 0 ], 86 | [ 0, 1.0, 0 ], 87 | [ 0, 0, 1.0 ]], 88 | "Q": [[ 0.30901699437494706, 0, -0.9510565162951535 ], 89 | [ -0.9510565162951535, 0, -0.30901699437494695 ], 90 | [ 0, 0.9999999999999998, 0 ]], 91 | "translation": [ 1.7887194354762759, 10.67522752830821, 0 ] 92 | }, 93 | {"name": "theta: 360", 94 | "width": 640, 95 | "height": 480, 96 | "P": [[ 300.0, 0, 320.0, 0 ], 97 | [ 0, 300.0, 240.0, 0 ], 98 | [ 0, 0, 1.0, 0 ]], 99 | "K": [[ 300.0, 0, 320.0 ], 100 | [ 0, 300.0, 240.0 ], 101 | [ 0, 0, 1.0 ]], 102 | "D": [ 0, 0, 0, 0, 0 ], 103 | "R": [[ 1.0, 0, 0 ], 104 | [ 0, 1.0, 0 ], 105 | [ 0, 0, 1.0 ]], 106 | "Q": [[ 1.0, 0, 0 ], 107 | [ 0, 0, -1.0000000000000002 ], 108 | [ 0, 1.0000000000000002, 0 ]], 109 | "translation": [ -11.999999999999998, 5.000000000000005, 1.0364959731544977e-15 ] 110 | } 111 | ] 112 | } -------------------------------------------------------------------------------- /tests/test_align.py: -------------------------------------------------------------------------------- 1 | from __future__ import print_function 2 | from pymvg.align import estsimt, align_points, align_M 3 | import numpy as np 4 | 5 | def test_align(): 6 | orig_points = np.array([ 7 | [3.36748406, 1.61036404, 3.55147255], 8 | [3.58702265, 0.06676394, 3.64695356], 9 | [0.28452026, -0.11188296, 3.78947735], 10 | [0.25482713, 1.57828256, 3.6900808], 11 | 12 | [3.54938525, 1.74057692, 5.13329681], 13 | [3.6855626 , 0.10335229, 5.26344841], 14 | [0.25025385, -0.06146044, 5.57085135], 15 | [0.20742481, 1.71073272, 5.41823085], 16 | ]).T 17 | 18 | ft2inch = 12.0 19 | inch2cm = 2.54 20 | cm2m = 0.01 21 | ft2m = ft2inch * inch2cm * cm2m 22 | 23 | x1,y1,z1=0,0,0 24 | x2,y2,z2=np.array([10,5,5])*ft2m 25 | 26 | new_points = np.array([ 27 | [x2, y2, z2], 28 | [x2, y1, z2], 29 | [x1, y1, z2], 30 | [x1, y2, z2], 31 | 32 | [x2, y2, z1], 33 | [x2, y1, z1], 34 | [x1, y1, z1], 35 | [x1, y2, z1], 36 | ]).T 37 | 38 | print(orig_points.T) 39 | print(new_points.T) 40 | 41 | s,R,t = estsimt(orig_points,new_points) 42 | print('s=%s'%repr(s)) 43 | print('R=%s'%repr(R.tolist())) 44 | print('t=%s'%repr(t.tolist())) 45 | Xnew = align_points( s,R,t, orig_points ) 46 | 47 | # measure distance between elements 48 | mean_absdiff = np.mean( abs(Xnew[:3]-new_points).flatten() ) 49 | assert mean_absdiff < 0.05 50 | 51 | M_orig = np.array([[1,2,3,4], 52 | [5,6,7,8], 53 | [9,10,11,12]],dtype=float) 54 | print('Xnew.T') 55 | print(Xnew.T) 56 | 57 | M_new = align_M( s,R,t, M_orig ) 58 | print('M_new') 59 | print(M_new) 60 | 61 | ## print('s',s) 62 | ## print('R') 63 | ## print(R) 64 | ## print('T') 65 | ## print(T) 66 | -------------------------------------------------------------------------------- /tests/test_camera_model.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import os, tempfile 5 | import pickle 6 | 7 | from pymvg.camera_model import CameraModel 8 | from pymvg.util import point_msg_to_tuple, parse_rotation_msg 9 | import pymvg.align as mcsc_align 10 | 11 | from tests.utils import _build_test_camera, get_default_options 12 | from unittest import SkipTest 13 | import pytest 14 | 15 | def pytest_generate_tests(metafunc): 16 | if "cam_opts" in metafunc.fixturenames: 17 | all_options = get_default_options() 18 | metafunc.parametrize("cam_opts", all_options) 19 | if "simple_cam_opts" in metafunc.fixturenames: 20 | all_options = [] 21 | at_origin=True # this test mathematically only makes sense of camera at origin 22 | for ROS_test_data in (True,False): 23 | opts = dict(at_origin=at_origin,ROS_test_data=ROS_test_data) 24 | all_options.append(opts) 25 | metafunc.parametrize("simple_cam_opts", all_options) 26 | if "distorted" in metafunc.fixturenames: 27 | metafunc.parametrize("distorted", [True, False]) 28 | if "axis" in metafunc.fixturenames: 29 | metafunc.parametrize("axis", ["lr", "ud"]) 30 | 31 | # --------------------- testing ----------------------------- 32 | 33 | def _generate_uv_raw(width,height): 34 | step = 5 35 | border = 65 36 | 37 | uv_raws = [] 38 | for row in range(border, height-border, step): 39 | for col in range(border, width-border, step): 40 | uv_raw = [col, row] 41 | uv_raws.append(uv_raw) 42 | return np.array(uv_raws) 43 | 44 | def test_dict_roundtrip(cam_opts): 45 | cam = _build_test_camera(**cam_opts) 46 | d = cam.to_dict() 47 | cam2 = CameraModel.from_dict(d) 48 | assert cam==cam2 49 | 50 | def test_projection_to_undistorted1(simple_cam_opts): 51 | """check that points along optical axis are imaged onto principal point""" 52 | cam = _build_test_camera(**simple_cam_opts) 53 | for z in np.linspace(0.1, 10, 20): 54 | pt = np.array([[0,0,z]]) 55 | result = cam.project_3d_to_pixel( pt, distorted=False ) 56 | u,v = result[0] 57 | 58 | assert np.allclose(u, cam.P[0,2]) 59 | assert np.allclose(v, cam.P[1,2]) 60 | 61 | def test_camera_distortion_roundtrip(cam_opts): 62 | """check that uv == distort( undistort( uv ))""" 63 | cam = _build_test_camera(**cam_opts) 64 | uv_raw = _generate_uv_raw(cam.width, cam.height) 65 | uv_rect = cam.undistort( uv_raw ) 66 | uv_unrect = cam.distort( uv_rect ) 67 | assert uv_raw.shape == uv_unrect.shape 68 | assert np.allclose(uv_raw, uv_unrect, atol=1.0) # within one pixel 69 | 70 | def test_camera_projection_roundtrip(cam_opts,distorted): 71 | """check that uv == project_to_2d( project_to_3d( uv ))""" 72 | cam = _build_test_camera(**cam_opts) 73 | uv_raw = _generate_uv_raw(cam.width, cam.height) 74 | 75 | pts3d = cam.project_pixel_to_3d_ray( uv_raw, distorted=distorted ) 76 | uv_unrect = cam.project_3d_to_pixel( pts3d, distorted=distorted ) 77 | assert uv_raw.shape == uv_unrect.shape 78 | assert np.allclose(uv_raw, uv_unrect, atol=1.0) # within one pixel 79 | 80 | def test_extrinsic_msg(cam_opts): 81 | """check that ROS message contains actual camera extrinsic parameters""" 82 | cam_opts = cam_opts.copy() 83 | cam_opts['get_input_data']=True 84 | r = _build_test_camera(**cam_opts) 85 | cam = r['cam'] 86 | tfm = cam.get_extrinsics_as_bunch() 87 | if 'translation' in r: 88 | assert np.allclose(point_msg_to_tuple(tfm.translation), point_msg_to_tuple(r['translation'])) 89 | if 'rotation' in r: 90 | assert np.allclose(parse_rotation_msg(tfm.rotation,force_matrix=True), 91 | parse_rotation_msg(r['rotation'],force_matrix=True)) 92 | 93 | def test_built_from_M(cam_opts): 94 | """check that M is preserved in load_camera_from_M() factory""" 95 | cam_orig = _build_test_camera(**cam_opts) 96 | M_orig = cam_orig.get_M() 97 | cam = CameraModel.load_camera_from_M( M_orig ) 98 | assert np.allclose( cam.get_M(), M_orig) 99 | 100 | def test_align(cam_opts): 101 | cam_orig = _build_test_camera(**cam_opts) 102 | M_orig = cam_orig.get_M() 103 | cam_orig = CameraModel.load_camera_from_M( M_orig ) 104 | R1 = np.eye(3) 105 | R2 = np.zeros((3,3)) 106 | R2[0,1] = 1 107 | R2[1,0] = 1 108 | R2[2,2] = -1 109 | t1 = np.array( (0.0, 0.0, 0.0) ) 110 | t2 = np.array( (0.0, 0.0, 0.1) ) 111 | t3 = np.array( (0.1, 0.0, 0.0) ) 112 | for s in [1.0, 2.0]: 113 | for R in [R1, R2]: 114 | for t in [t1, t2, t3]: 115 | cam_actual = cam_orig.get_aligned_camera( s, R, t ) 116 | M_expected = mcsc_align.align_M( s,R,t, M_orig ) 117 | cam_expected = CameraModel.load_camera_from_M( M_expected ) 118 | assert cam_actual==cam_expected 119 | 120 | def test_problem_M(): 121 | """check a particular M which previously caused problems""" 122 | # This M (found by the DLT method) was causing me problems. 123 | d = {'width': 848, 124 | 'name': 'camera', 125 | 'height': 480} 126 | M = np.array([[ -1.70677031e+03, -4.10373295e+03, -3.88568028e+02, 6.89034515e+02], 127 | [ -6.19019195e+02, -1.01292091e+03, -2.67534989e+03, 4.51847857e+02], 128 | [ -4.52548832e+00, -3.78900498e+00, -7.35860226e-01, 1.00000000e+00]]) 129 | cam = CameraModel.load_camera_from_M( M, **d) 130 | 131 | #assert np.allclose( cam.M, M) # we don't expect this since the intrinsic matrix may not be scaled 132 | 133 | verts = np.array([[ 0.042306, 0.015338, 0.036328, 1.0], 134 | [ 0.03323, 0.030344, 0.041542, 1.0], 135 | [ 0.036396, 0.026464, 0.052408, 1.0]]) 136 | 137 | actual = cam.project_3d_to_pixel(verts[:,:3]) 138 | 139 | expectedh = np.dot( M, verts.T ) 140 | expected = (expectedh[:2]/expectedh[2]).T 141 | assert np.allclose( expected, actual ) 142 | 143 | def test_distortion_yamlfile_roundtrip(cam_opts): 144 | """check that roundtrip of camera model to/from a yaml file works""" 145 | cam = _build_test_camera(**cam_opts) 146 | fname = tempfile.mktemp(suffix='.yaml') 147 | cam.save_intrinsics_to_yamlfile(fname) 148 | try: 149 | cam2 = CameraModel.load_camera_from_file( fname, extrinsics_required=False ) 150 | finally: 151 | os.unlink(fname) 152 | 153 | distorted = np.array( [[100.0,100], 154 | [100,200], 155 | [100,300], 156 | [100,400]] ) 157 | orig_undistorted = cam.undistort( distorted ) 158 | reloaded_undistorted = cam2.undistort( distorted ) 159 | assert np.allclose( orig_undistorted, reloaded_undistorted ) 160 | 161 | def test_camera_mirror_projection_roundtrip(cam_opts,distorted,axis): 162 | """check that a mirrored camera gives reflected pixel coords""" 163 | cam_orig = _build_test_camera(**cam_opts) 164 | cam_mirror = cam_orig.get_mirror_camera(axis=axis) 165 | uv_raw = _generate_uv_raw(cam_orig.width, cam_orig.height) 166 | 167 | # Get a collection of 3D points for which we know the pixel location of 168 | pts3d = cam_orig.project_pixel_to_3d_ray( uv_raw, distorted=distorted ) 169 | # Now project that through our mirror-image camera. 170 | uv_mirror = cam_mirror.project_3d_to_pixel( pts3d, distorted=distorted ) 171 | # Which points should be xnew = (width-x) 172 | expected = np.array(uv_raw) 173 | if axis=='lr': 174 | expected[:,0] = cam_orig.width - uv_raw[:,0] 175 | else: 176 | expected[:,1] = cam_orig.height - uv_raw[:,1] 177 | assert expected.shape == uv_mirror.shape 178 | assert np.allclose(expected, uv_mirror, atol=1.0) # within one pixel 179 | 180 | def test_flip(cam_opts): 181 | cam_orig = _build_test_camera(**cam_opts) 182 | try: 183 | cam_flip = cam_orig.get_flipped_camera() 184 | except NotImplementedError as err: 185 | raise SkipTest(str(err)) 186 | 187 | # They have different orientation (but same position) in space, 188 | assert not np.allclose( cam_orig.get_rotation(), cam_flip.get_rotation()) 189 | assert np.allclose( cam_orig.get_camcenter(), cam_flip.get_camcenter()) 190 | 191 | eye, lookat, up = cam_orig.get_view() 192 | eye2, lookat2, up2 = cam_flip.get_view() 193 | 194 | assert not np.allclose( lookat, lookat2 ) 195 | 196 | # but they project 3D points to same pixel locations 197 | verts = np.array([[ 0.042306, 0.015338, 0.036328], 198 | [ 1.03323, 2.030344, 3.041542], 199 | [ 0.03323, 0.030344, 0.041542], 200 | [ 0.036396, 0.026464, 0.052408]]) 201 | 202 | expected = cam_orig.project_3d_to_pixel(verts) 203 | actual = cam_flip.project_3d_to_pixel(verts) 204 | assert np.allclose( expected, actual ) 205 | 206 | def test_view(cam_opts): 207 | """check that we can reset camera extrinsic parameters""" 208 | 209 | # This is not a very good test. (Should maybe check more eye 210 | # positions, more lookat directions, and more up vectors.) 211 | 212 | cam_orig = _build_test_camera(**cam_opts) 213 | eye = (10,20,30) 214 | lookat = (11,20,30) # must be unit length for below to work 215 | up = (0,-1,0) 216 | cam_new = cam_orig.get_view_camera(eye, lookat, up) 217 | eye2, lookat2, up2 = cam_new.get_view() 218 | assert np.allclose( eye, eye2) 219 | assert np.allclose( lookat, lookat2 ) 220 | assert np.allclose( up, up2 ) 221 | 222 | # check a case that was previously failing 223 | n=6 224 | x = np.linspace(0, 2*n, n) 225 | theta = np.linspace(0, 2*np.pi, n) 226 | dim = 5.0 227 | for i in range(n): 228 | center = np.array( (x[i], 0.0, dim) ) 229 | lookat = center + np.array( (0,1,0)) 230 | up = -np.sin(theta[i]), 0, np.cos(theta[i]) 231 | 232 | cam_new2 = cam_orig.get_view_camera(eye=center, lookat=lookat) 233 | 234 | # check a pathological case 235 | center= [ 0., 0., 5.] 236 | lookat= [ 0., 1., 5.] 237 | up = [0,-1,0] 238 | try: 239 | cam_new3 = cam_orig.get_view_camera(eye=center, lookat=lookat, up=up) 240 | except AssertionError as err: 241 | # we should get this exception 242 | pass 243 | else: 244 | assert 1==0, "did not fail test" 245 | 246 | def test_camcenter(cam_opts): 247 | """check that our idea of camera center is theoretically expected value""" 248 | cam = _build_test_camera(**cam_opts) 249 | assert np.allclose( cam.get_camcenter(), cam.get_t_inv().T ) 250 | 251 | def test_stages(cam_opts, distorted): 252 | """check the sum of all stages equals all stages summed""" 253 | cam = _build_test_camera(**cam_opts) 254 | 255 | uv_raw = _generate_uv_raw(cam.width, cam.height) 256 | pts3d = cam.project_pixel_to_3d_ray( uv_raw, distorted=distorted ) 257 | 258 | # case 1: direct projection to pixels 259 | direct = cam.project_3d_to_pixel( pts3d, distorted=distorted ) 260 | 261 | # case 2: project to camera frame, then to pixels 262 | cam_frame = cam.project_3d_to_camera_frame(pts3d) 263 | indirect = cam.project_camera_frame_to_pixel(cam_frame, distorted=distorted) 264 | 265 | assert np.allclose(direct, indirect) 266 | 267 | def test_simple_camera(): 268 | center = np.array( (0, 0.0, 5) ) 269 | lookat = center + np.array( (0,1,0)) 270 | cam = CameraModel.load_camera_simple(fov_x_degrees=90, 271 | eye=center, 272 | lookat=lookat) 273 | 274 | def test_equality(): 275 | center = np.array( (0, 0.0, 5) ) 276 | lookat = center + np.array( (0,1,0)) 277 | cam_apple1 = CameraModel.load_camera_simple(fov_x_degrees=90, 278 | eye=center, 279 | lookat=lookat, 280 | name='apple') 281 | cam_apple2 = CameraModel.load_camera_simple(fov_x_degrees=90, 282 | eye=center, 283 | lookat=lookat, 284 | name='apple') 285 | cam_orange = CameraModel.load_camera_simple(fov_x_degrees=30, 286 | eye=center, 287 | lookat=lookat, 288 | name='orange') 289 | assert cam_apple1==cam_apple2 290 | assert cam_apple1!=cam_orange 291 | assert not cam_apple1==cam_orange 292 | 293 | def test_pickle_roundtrip(cam_opts): 294 | cam = _build_test_camera(**cam_opts) 295 | buf = pickle.dumps(cam) 296 | cam2 = pickle.loads(buf) 297 | assert cam==cam2 298 | 299 | def test_camcenter_like(cam_opts): 300 | cam = _build_test_camera(**cam_opts) 301 | cc_expected = cam.get_camcenter() 302 | for n in range(4): 303 | nparr = np.zeros( (n,3), dtype=float ) 304 | cc = cam.camcenter_like( nparr ) 305 | for i in range(n): 306 | this_cc = cc[i] 307 | assert np.allclose(cc_expected,this_cc) 308 | -------------------------------------------------------------------------------- /tests/test_dlt.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import numpy as np 3 | 4 | import pymvg.calibration 5 | import pymvg.camera_model 6 | 7 | def test_dlt_old_data(): 8 | # some old real data 9 | X3d = np.array([[ 304.8, 0. , 0. ], 10 | [ 304.8, 152.4, 0. ], 11 | [ 304.8, 152.4, 152.4], 12 | [ 304.8, 0. , 152.4], 13 | [ 178. , 85. , 86. ], 14 | [ 178. , 85. , 63. ]]) 15 | x2d_orig = np.array([[ 120. , 475. ], 16 | [ 522. , 460. ], 17 | [ 497. , 69.6], 18 | [ 120. , 76.2], 19 | [ 344. , 200. ], 20 | [ 349. , 298. ]]) 21 | 22 | results = pymvg.calibration.DLT(X3d, x2d_orig) 23 | cam=results['cam'] 24 | x2d_reproj = cam.project_3d_to_pixel(X3d) 25 | 26 | # calculate point-by-point reprojection error 27 | err = np.sqrt(np.sum( (x2d_orig - x2d_reproj)**2, axis=1 )) 28 | 29 | # find mean reprojection error across all pixels 30 | mean_err = np.mean(err) 31 | assert mean_err < 3.0 32 | 33 | def test_dlt_roundtrip(): 34 | eye = (10,20,30) 35 | lookat = (11,20,30) 36 | up = (1,-1,0) 37 | # must be unit length for below to work 38 | 39 | n_pts = 20 40 | theta = np.linspace(0,2*np.pi*3,n_pts) 41 | z = np.linspace(-1,1,n_pts) 42 | scale = 0.5 43 | pts_3d = np.array( [ scale*np.cos(theta), 44 | scale*np.sin(theta), 45 | scale*z ] ).T 46 | pts_3d += np.array(lookat) 47 | 48 | cam_orig = pymvg.camera_model.CameraModel.load_camera_default() 49 | cam = cam_orig.get_view_camera(eye, lookat, up) 50 | 51 | x2d_orig = cam.project_3d_to_pixel(pts_3d) 52 | 53 | results = pymvg.calibration.DLT(pts_3d,x2d_orig, width=cam.width, height=cam.height) 54 | 55 | cam_new=results['cam'] 56 | x2d_reproj = cam_new.project_3d_to_pixel(pts_3d) 57 | 58 | # calculate point-by-point reprojection error 59 | err = np.sqrt(np.sum( (x2d_orig - x2d_reproj)**2, axis=1 )) 60 | 61 | # find mean reprojection error across all pixels 62 | mean_err = np.mean(err) 63 | assert mean_err < 0.001 64 | -------------------------------------------------------------------------------- /tests/test_first_principles.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | from tests.utils import _build_points_3d, make_M 5 | import os 6 | 7 | from pymvg.util import normalize 8 | from pymvg.camera_model import CameraModel 9 | 10 | DRAW=int(os.environ.get('DRAW','0')) 11 | if DRAW: 12 | import matplotlib.pyplot as plt 13 | from mpl_toolkits.mplot3d import Axes3D 14 | from pymvg.plot_utils import plot_camera 15 | 16 | def pytest_generate_tests(metafunc): 17 | if "distorted" in metafunc.fixturenames: 18 | metafunc.parametrize("distorted", [True, False]) 19 | 20 | def test_lookat(): 21 | 22 | dist = 5.0 23 | 24 | # build camera 25 | center_expected = np.array( [10, 5, 20] ) 26 | lookat_expected = center_expected + np.array( [dist, 0, 0] ) # looking in +X 27 | up_expected = np.array( [0, 0, 1] ) 28 | 29 | f = 300.0 # focal length 30 | width, height = 640, 480 31 | cx, cy = width/2.0, height/2.0 32 | 33 | M = np.array( [[ f, 0, cx, 0], 34 | [ 0, f, cy, 0], 35 | [ 0, 0, 1, 0]]) 36 | cam1 = CameraModel.load_camera_from_M( M, width=width, height=height) 37 | cam = cam1.get_view_camera(center_expected, lookat_expected, up_expected) 38 | del cam1 39 | 40 | # check that the extrinsic parameters were what we expected 41 | (center_actual,lookat_actual,up_actual) = cam.get_view() 42 | 43 | lookdir_expected = normalize( lookat_expected - center_expected ) 44 | lookdir_actual = normalize( lookat_actual - center_actual ) 45 | 46 | assert np.allclose( center_actual, center_expected ) 47 | assert np.allclose( lookdir_actual, lookdir_expected ) 48 | assert np.allclose( up_actual, up_expected ) 49 | 50 | # check that the extrinsics work as expected 51 | pts = np.array([lookat_expected, 52 | lookat_expected+up_expected]) 53 | eye_actual = cam.project_3d_to_camera_frame( pts ) 54 | 55 | eye_expected = [[0, 0, dist], # camera looks at +Z 56 | [0,-1, dist], # with -Y as up 57 | ] 58 | assert np.allclose( eye_actual, eye_expected ) 59 | 60 | # now check some basics of the projection 61 | pix_actual = cam.project_3d_to_pixel( pts ) 62 | 63 | pix_expected = [[cx,cy], # center pixel on the camera 64 | [cx,cy-(f/dist)]] 65 | assert np.allclose( pix_actual, pix_expected ) 66 | 67 | def test_flip(distorted): 68 | if distorted: 69 | d = [0.1, 0.2, 0.3, 0.4, 0.5] 70 | else: 71 | d = None 72 | 73 | # build camera 74 | center_expected = np.array( [10, 5, 20] ) 75 | lookat_expected = center_expected + np.array( [1, 2, 0] ) 76 | up_expected = np.array( [0, 0, 1] ) 77 | 78 | width, height = 640, 480 79 | 80 | M = np.array( [[ 300.0, 0, 321, 0], 81 | [ 0, 298.0, 240, 0], 82 | [ 0, 0, 1, 0]]) 83 | cam1 = CameraModel.load_camera_from_M( M, width=width, height=height, 84 | distortion_coefficients=d ) 85 | cam = cam1.get_view_camera(center_expected, lookat_expected, up_expected) 86 | del cam1 87 | 88 | pts = np.array([lookat_expected, 89 | lookat_expected+up_expected, 90 | [1,2,3], 91 | [4,5,6]]) 92 | pix_actual = cam.project_3d_to_pixel( pts ) 93 | 94 | # Flipped camera gives same 3D->2D transform but different look direction. 95 | cf = cam.get_flipped_camera() 96 | assert not np.allclose( cam.get_lookat(), cf.get_lookat() ) 97 | 98 | pix_actual_flipped = cf.project_3d_to_pixel( pts ) 99 | assert np.allclose( pix_actual, pix_actual_flipped ) 100 | 101 | def test_simple_projection(): 102 | 103 | # get some 3D points 104 | pts_3d = _build_points_3d() 105 | 106 | if DRAW: 107 | fig = plt.figure(figsize=(8,12)) 108 | ax1 = fig.add_subplot(3,1,1, projection='3d') 109 | ax1.scatter( pts_3d[:,0], pts_3d[:,1], pts_3d[:,2]) 110 | ax1.set_xlabel('X') 111 | ax1.set_ylabel('Y') 112 | ax1.set_zlabel('Z') 113 | 114 | # build a camera calibration matrix 115 | focal_length = 1200 116 | width, height = 640,480 117 | R = np.eye(3) # look at +Z 118 | c = np.array( (9.99, 19.99, 20) ) 119 | M = make_M( focal_length, width, height, R, c)['M'] 120 | 121 | # now, project these 3D points into our image plane 122 | pts_3d_H = np.vstack( (pts_3d.T, np.ones( (1,len(pts_3d))))) # make homog. 123 | undist_rst_simple = np.dot(M, pts_3d_H) # multiply 124 | undist_simple = undist_rst_simple[:2,:]/undist_rst_simple[2,:] # project 125 | 126 | if DRAW: 127 | ax2 = fig.add_subplot(3,1,2) 128 | ax2.plot( undist_simple[0,:], undist_simple[1,:], 'b.') 129 | ax2.set_xlim(0,width) 130 | ax2.set_ylim(height,0) 131 | ax2.set_title('matrix multiply') 132 | 133 | # build a camera model from our M and project onto image plane 134 | cam = CameraModel.load_camera_from_M( M, width=width, height=height ) 135 | undist_full = cam.project_3d_to_pixel(pts_3d).T 136 | 137 | if DRAW: 138 | plot_camera( ax1, cam, scale=10, axes_size=5.0 ) 139 | sz = 20 140 | x = 5 141 | y = 8 142 | z = 19 143 | ax1.auto_scale_xyz( [x,x+sz], [y,y+sz], [z,z+sz] ) 144 | 145 | ax3 = fig.add_subplot(3,1,3) 146 | ax3.plot( undist_full[0,:], undist_full[1,:], 'b.') 147 | ax3.set_xlim(0,width) 148 | ax3.set_ylim(height,0) 149 | ax3.set_title('pymvg') 150 | 151 | if DRAW: 152 | plt.show() 153 | 154 | assert np.allclose( undist_full, undist_simple ) 155 | 156 | if __name__=='__main__': 157 | test_simple_projection() 158 | test_lookat() 159 | -------------------------------------------------------------------------------- /tests/test_multi_camera_system.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import pymvg 3 | from pymvg.camera_model import CameraModel 4 | from pymvg.multi_camera_system import MultiCameraSystem, build_example_system 5 | import tempfile, os 6 | import pytest 7 | 8 | def make_default_system(): 9 | '''helper function to generate an instance of MultiCameraSystem''' 10 | lookat = np.array( (0.0, 0.0, 0.0)) 11 | 12 | center1 = np.array( (0.0, 0.0, 5.0) ) 13 | distortion1 = np.array( [0.2, 0.3, 0.1, 0.1, 0.1] ) 14 | cam1 = CameraModel.load_camera_simple(name='cam1', 15 | fov_x_degrees=90, 16 | eye=center1, 17 | lookat=lookat, 18 | distortion_coefficients=distortion1, 19 | ) 20 | 21 | center2 = np.array( (0.5, 0.0, 0.0) ) 22 | cam2 = CameraModel.load_camera_simple(name='cam2', 23 | fov_x_degrees=90, 24 | eye=center2, 25 | lookat=lookat, 26 | ) 27 | 28 | center3 = np.array( (0.5, 0.5, 0.0) ) 29 | cam3 = CameraModel.load_camera_simple(name='cam3', 30 | fov_x_degrees=90, 31 | eye=center3, 32 | lookat=lookat, 33 | ) 34 | 35 | center4 = np.array( (0.5, 0.0, 0.5) ) 36 | cam4 = CameraModel.load_camera_simple(name='cam4', 37 | fov_x_degrees=90, 38 | eye=center4, 39 | lookat=lookat, 40 | ) 41 | 42 | cameras = [cam1,cam2,cam3,cam4] 43 | system = MultiCameraSystem(cameras) 44 | return system 45 | 46 | def get_default_points(): 47 | pts_3d = [ (0.0, 0.0, 0.0), 48 | (0.1, 0.0, 0.0), 49 | (0.0, 0.1, 0.0), 50 | (0.0, 0.0, 0.1), 51 | (0.1, 0.1, 0.0), 52 | (0.1, 0.0, 0.1), 53 | (0.0, 0.1, 0.1), 54 | ] 55 | return np.array(pts_3d) 56 | 57 | def test_roundtrip(): 58 | system = make_default_system() 59 | pts_3d = get_default_points() 60 | for expected in pts_3d: 61 | pts = [] 62 | for name in system.get_names(): 63 | pt2d = system.find2d( name, expected ) 64 | tup = (name, pt2d) 65 | pts.append(tup) 66 | 67 | actual = system.find3d( pts ) 68 | assert np.allclose( expected, actual ) 69 | 70 | def test_no_duplicate_names(): 71 | cam1a = CameraModel.load_camera_simple(name='cam1') 72 | cam1b = CameraModel.load_camera_simple(name='cam1') 73 | cams = [cam1a,cam1b] 74 | pytest.raises(ValueError, MultiCameraSystem, cams) 75 | 76 | def test_equals(): 77 | system = make_default_system() 78 | assert system != 1234 79 | 80 | system2 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(2)]) 81 | system3 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(3)]) 82 | assert system2 != system3 83 | 84 | system4 = make_default_system() 85 | d = system4.to_dict() 86 | d['camera_system'][0]['width'] += 1 87 | system5 = MultiCameraSystem.from_dict( d ) 88 | assert system4 != system5 89 | 90 | def test_single_and_multiple_points_find2d(): 91 | '''ensure that find2d works with single points and multiple points''' 92 | system = make_default_system() 93 | pts_3d = get_default_points() 94 | 95 | name = system.get_names()[0] 96 | pt_3d = pts_3d[0] 97 | 98 | single_2d = system.find2d( name, pt_3d ) 99 | assert single_2d.ndim==1 100 | 101 | multiple_2d = system.find2d( name, pts_3d ) 102 | assert multiple_2d.ndim==2 103 | assert multiple_2d.shape[0]==2 104 | assert multiple_2d.shape[1]==pts_3d.shape[0] 105 | 106 | def test_roundtrip_to_dict(): 107 | system1 = make_default_system() 108 | system2 = MultiCameraSystem.from_dict( system1.to_dict() ) 109 | assert system1==system2 110 | 111 | def test_getters(): 112 | system1 = make_default_system() 113 | d = system1.to_dict() 114 | names1 = list(system1.get_camera_dict().keys()) 115 | names2 = [cd['name'] for cd in d['camera_system']] 116 | assert set(names1)==set(names2) 117 | 118 | for idx in range(len(names1)): 119 | cam1 = system1.get_camera( names1[idx] ) 120 | cam2 = CameraModel.from_dict(d['camera_system'][idx]) 121 | assert cam1==cam2 122 | 123 | def test_roundtrip_to_pymvg_file(): 124 | system1 = make_default_system() 125 | fname = tempfile.mktemp(suffix='.json') 126 | system1.save_to_pymvg_file( fname ) 127 | try: 128 | system2 = MultiCameraSystem.from_pymvg_file( fname ) 129 | assert system1==system2 130 | finally: 131 | os.unlink( fname ) 132 | 133 | def test_pymvg_file_in_docs(): 134 | # Keep in sync with docs/source/pymvg_camsystem_example.json 135 | fname = 'tests/pymvg_camsystem_example.json' 136 | system = MultiCameraSystem.from_pymvg_file( fname ) 137 | 138 | def test_roundtrip_to_str(): 139 | system1 = make_default_system() 140 | buf = system1.get_pymvg_str() 141 | system2 = MultiCameraSystem.from_pymvg_str( buf ) 142 | assert system1==system2 143 | 144 | def test_align(): 145 | system1 = make_default_system() 146 | system2 = system1.get_aligned_copy( system1 ) # This should be a no-op. 147 | assert system1==system2 148 | 149 | def test_align(): 150 | system1 = make_default_system() 151 | system2 = system1.get_aligned_copy( system1 ) # This should be a no-op. 152 | assert system1==system2 153 | 154 | system3 = MultiCameraSystem([CameraModel.load_camera_simple(name='cam%d'%i) for i in range(2)]) 155 | pytest.raises(ValueError, system3.get_aligned_copy, system1) 156 | 157 | def test_build_example_system(): 158 | for n in range(2,100,5): 159 | system = build_example_system(n=n) 160 | assert n==len(system.get_names()) 161 | 162 | def test_load_mcsc(): 163 | mydir = os.path.dirname(__file__) 164 | mcsc_dir = os.path.join(mydir,'external','mcsc') 165 | mcsc_dirname = os.path.join(mcsc_dir,'mcsc_output_20130726') 166 | cam_system = MultiCameraSystem.from_mcsc( mcsc_dirname ) 167 | -------------------------------------------------------------------------------- /tests/utils.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import numpy as np 4 | import os 5 | 6 | from pymvg.camera_model import CameraModel 7 | from pymvg.multi_camera_system import MultiCameraSystem 8 | from pymvg.util import point_msg_to_tuple, parse_rotation_msg 9 | from pymvg.ros_compat import geometry_msgs, sensor_msgs 10 | 11 | def make_M( focal_length, width, height, R, c): 12 | K = np.eye(3) 13 | K[0,0] = focal_length 14 | K[1,1] = focal_length 15 | K[0,2] = width/2.0 16 | K[1,2] = height/2.0 17 | C = np.array(c,copy=True) 18 | C.shape = (3,1) 19 | t = -np.dot( R, C) 20 | M = np.dot(K, np.hstack((R,t))) 21 | parts = {'K':K, 'R':R, 't':t, 'M':M} 22 | return parts 23 | 24 | def _build_opts(): 25 | opts = [] 26 | for at_origin in (True,False): 27 | for ROS_test_data in (True,False): 28 | #for flipped in (True,False): 29 | for flipped in (False,): 30 | opts.append(dict(at_origin=at_origin, 31 | ROS_test_data=ROS_test_data, 32 | flipped=flipped)) 33 | # data from a ROS yaml file 34 | test_dir = os.path.split( __file__ )[0] 35 | yaml_base_fname = 'roscal.yaml' 36 | yaml_fname = os.path.join( test_dir, yaml_base_fname ) 37 | for at_origin in [True,False]: 38 | opts.append({'from_file':True, 39 | 'type':'ros', 40 | 'filename':yaml_fname, 41 | 'at_origin':at_origin, 42 | 'flipped':False}) 43 | 44 | # a PyMVG file with skewed pixels 45 | test_dir = os.path.split( __file__ )[0] 46 | pymvg_base_fname = 'skew_pixels_no_distortion.json' 47 | pymvg_fname = os.path.join( test_dir, pymvg_base_fname ) 48 | opts.append({'from_file':True, 49 | 'type':'pymvg', 50 | 'filename':pymvg_fname, 51 | } 52 | ) 53 | 54 | # a PyMVG file with skewed, distorted pixels 55 | test_dir = os.path.split( __file__ )[0] 56 | pymvg_base_fname = 'skew_pixels.json' 57 | pymvg_fname = os.path.join( test_dir, pymvg_base_fname ) 58 | opts.append({'from_file':True, 59 | 'type':'pymvg', 60 | 'filename':pymvg_fname, 61 | } 62 | ) 63 | 64 | # a PyMVG file generated synthetically (this was giving me trouble with rotations) 65 | test_dir = os.path.split( __file__ )[0] 66 | pymvg_base_fname = 'synthetic.json' 67 | pymvg_fname = os.path.join( test_dir, pymvg_base_fname ) 68 | opts.append({'from_file':True, 69 | 'type':'pymvg', 70 | 'filename':pymvg_fname, 71 | } 72 | ) 73 | return opts 74 | opts = _build_opts() 75 | 76 | class Bunch: 77 | def __init__(self, **kwds): 78 | self.__dict__.update(kwds) 79 | 80 | def _build_test_camera(**kwargs): 81 | o = Bunch(**kwargs) 82 | if not kwargs.get('at_origin',False): 83 | translation = geometry_msgs.msg.Point() 84 | translation.x = 0.273485679077 85 | translation.y = 0.0707310128808 86 | translation.z = 0.0877802104531 87 | 88 | rotation = geometry_msgs.msg.Quaternion() 89 | rotation.x = 0.309377331102 90 | rotation.y = 0.600893485738 91 | rotation.z = 0.644637681813 92 | rotation.w = 0.357288321925 93 | else: 94 | translation = geometry_msgs.msg.Point() 95 | translation.x = 0.0 96 | translation.y = 0.0 97 | translation.z = 0.0 98 | 99 | rotation = geometry_msgs.msg.Quaternion() 100 | rotation.x = 0.0 101 | rotation.y = 0.0 102 | rotation.z = 0.0 103 | rotation.w = 1.0 104 | 105 | if 'from_file' in kwargs: 106 | if kwargs['type']=='ros': 107 | cam = CameraModel.load_camera_from_file( fname=kwargs['filename'], 108 | extrinsics_required=False ) 109 | i = cam.get_intrinsics_as_bunch() 110 | cam = CameraModel._from_parts( 111 | translation=point_msg_to_tuple(translation), 112 | rotation=parse_rotation_msg(rotation), 113 | intrinsics=i, 114 | name='cam', 115 | ) 116 | elif kwargs['type']=='pymvg': 117 | del translation 118 | del rotation 119 | system = MultiCameraSystem.from_pymvg_file(fname=kwargs['filename']) 120 | names = system.get_names() 121 | 122 | names.sort() 123 | name = names[0] # get first camera from system 124 | cam = system._cameras[name] 125 | 126 | rotation = cam.get_extrinsics_as_bunch().rotation 127 | translation = cam.get_extrinsics_as_bunch().translation 128 | else: 129 | raise ValueError('unknown file type') 130 | 131 | else: 132 | 133 | if o.ROS_test_data: 134 | i = sensor_msgs.msg.CameraInfo() 135 | # these are from image_geometry ROS package in the utest.cpp file 136 | i.height = 480 137 | i.width = 640 138 | i.distortion_model = 'plumb_bob' 139 | i.D = [-0.363528858080088, 0.16117037733986861, -8.1109585007538829e-05, -0.00044776712298447841, 0.0] 140 | i.K = [430.15433020105519, 0.0, 311.71339830549732, 141 | 0.0, 430.60920415473657, 221.06824942698509, 142 | 0.0, 0.0, 1.0] 143 | i.R = [0.99806560714807102, 0.0068562422224214027, 0.061790256276695904, 144 | -0.0067522959054715113, 0.99997541519165112, -0.0018909025066874664, 145 | -0.061801701660692349, 0.0014700186639396652, 0.99808736527268516] 146 | i.P = [295.53402059708782, 0.0, 285.55760765075684, 0.0, 147 | 0.0, 295.53402059708782, 223.29617881774902, 0.0, 148 | 0.0, 0.0, 1.0, 0.0] 149 | else: 150 | i = sensor_msgs.msg.CameraInfo() 151 | i.height = 494 152 | i.width = 659 153 | i.distortion_model = 'plumb_bob' 154 | i.D = [-0.34146457767225, 0.196070795764995, 0.000548988393912233, 0.000657058395082583, -0.0828776806503243] 155 | i.K = [516.881868241566, 0.0, 333.090936517613, 0.0, 517.201263180996, 231.526036849886, 0.0, 0.0, 1.0] 156 | i.R = [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0] 157 | i.P = [442.17529296875, 0.0, 334.589001099812, 0.0, 0.0, 474.757141113281, 228.646131377705, 0.0, 0.0, 0.0, 1.0, 0.0] 158 | 159 | cam = CameraModel._from_parts( 160 | translation=point_msg_to_tuple(translation), 161 | rotation=parse_rotation_msg(rotation), 162 | intrinsics=i, 163 | name='cam', 164 | ) 165 | if kwargs.get('flipped',False): 166 | cam = cam.get_flipped_camera() 167 | if kwargs.get('get_input_data',False): 168 | return dict(cam=cam, 169 | translation=translation, 170 | rotation=rotation, 171 | ) 172 | return cam 173 | 174 | def _build_points_3d(): 175 | n_pts = 100 176 | x,y,z = 10.001, 20.001, 30.001 177 | theta = np.linspace(0,10,n_pts)%(2*np.pi) 178 | h = np.linspace(0,10,n_pts) 179 | r = 1.05 180 | pts3D = np.vstack( (r*np.cos(theta)+x, r*np.sin(theta)+y, h+z )).T 181 | return pts3D 182 | 183 | def get_default_options(): 184 | return [opts[i] for i in range(len(opts))] 185 | --------------------------------------------------------------------------------