diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index d23864fd8..e2394440d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -17,6 +17,7 @@ jobs: matrix: version: - '1.6' + - '^1.7.0-0' - 'nightly' os: - ubuntu-latest @@ -48,7 +49,7 @@ jobs: - uses: codecov/codecov-action@v1 with: file: lcov.info - + test-masters: #if: github.ref != 'refs/heads/release**' name: Upstream Dev @@ -57,12 +58,12 @@ jobs: JULIA_PKG_SERVER: "" steps: - uses: actions/checkout@v2 - + - uses: julia-actions/setup-julia@v1 with: version: 1.6 arch: x64 - + - uses: actions/cache@v1 env: cache-name: cache-artifacts @@ -73,7 +74,7 @@ jobs: ${{ runner.os }}-test-${{ env.cache-name }}- ${{ runner.os }}-test- ${{ runner.os }}- - + - run: | git config --global user.name Tester git config --global user.email te@st.er @@ -86,3 +87,34 @@ jobs: julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="master"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.test("Caesar"; coverage=false)' shell: bash + + docs: + name: 'Build Docs' + runs-on: ubuntu-latest + strategy: + matrix: + include: + - jlenv: 'docs/' + makejl: 'docs/make.jl' + # - jlenv: 'docs/pdf/' + # makejl: 'docs/pdf/make.jl' + steps: + - uses: actions/checkout@v2 + - uses: julia-actions/setup-julia@v1 + with: + version: '1' + - name: 'Docs on ${{ github.head_ref }}' + run: | + export JULIA_PKG_SERVER="" + [ '${{ github.ref }}' == 'refs/heads/master' ] && export CJL_DOCS_BRANCH="master" || export CJL_DOCS_BRANCH="${{ github.head_ref }}" + julia -e 'println("Julia gets branch: ",ENV["CJL_DOCS_BRANCH"])' + julia --project=${{ matrix.jlenv }} --check-bounds=yes -e 'using Pkg; Pkg.instantiate(); Pkg.add(PackageSpec(name="Caesar", rev=ENV["CJL_DOCS_BRANCH"]))' + julia --project=${{ matrix.jlenv }} -e 'using Pkg; Pkg.add(PackageSpec(name="RoME", rev="master"))' + julia --project=${{ matrix.jlenv }} -e 'using Pkg; Pkg.add(PackageSpec(name="RoMEPlotting", rev="master"))' + julia --project=${{ matrix.jlenv }} -e 'using Pkg; Pkg.add(PackageSpec(name="KernelDensityEstimatePlotting", rev="master"))' + julia --project=${{ matrix.jlenv }} -e 'using Pkg; Pkg.add(PackageSpec(name="IncrementalInference", rev="master"))' + - run: julia --project=${{ matrix.jlenv }} --color=yes ${{ matrix.makejl }} + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + DOCUMENTER_KEY: ${{ secrets.DOCUMENTER_KEY }} + JULIA_PKG_SERVER: "" diff --git a/.travis.yml b/.travis.yml deleted file mode 100644 index 6241f670a..000000000 --- a/.travis.yml +++ /dev/null @@ -1,54 +0,0 @@ -language: julia -sudo: required - -arch: - - amd64 - -os: - - linux - -env: - - JULIA_PKG_SERVER="" - -addons: - apt: - packages: - - hdf5-tools - -julia: - - 1.6 - - nightly - -jobs: - include: - - name: "Upstream Dev" - if: NOT branch =~ ^release.*$ - julia: 1.6 - script: - - if [[ -a .git/shallow ]]; then git fetch --unshallow; fi - - julia -e 'using Pkg; Pkg.add(PackageSpec(name="RoME", rev="master"))' - - julia -e 'using Pkg; Pkg.add(PackageSpec(name="IncrementalInference", rev="master"))' - - julia -e 'using Pkg; Pkg.add(PackageSpec(name="DistributedFactorGraphs", rev="master"))' - - julia --check-bounds=yes -e 'using Pkg; Pkg.test("Caesar"; coverage=false)' - after_success: skip - - stage: "Documentation" - julia: 1.6 - script: - - julia --project=docs/ -e 'using Pkg; Pkg.add(PackageSpec(name="Caesar", rev="master")); Pkg.instantiate()' - - julia --project=docs/ -e 'using Pkg; Pkg.add(PackageSpec(name="RoME", rev="master"))' - - julia --project=docs/ -e 'using Pkg; Pkg.add(PackageSpec(name="RoMEPlotting", rev="master"))' - - julia --project=docs/ -e 'using Pkg; Pkg.add(PackageSpec(name="KernelDensityEstimatePlotting", rev="master"))' - - julia --project=docs/ -e 'using Pkg; Pkg.add(PackageSpec(name="IncrementalInference", rev="master"))' - - julia --project=docs/ docs/make.jl - name: "HTML Docs" - after_success: skip - allow_failures: - - julia: nightly - fast_finish: true - -after_success: - - julia --project=coverage/ -e 'using Pkg; Pkg.instantiate()' - - julia --project=coverage/ coverage/coverage.jl - -notifications: - email: false diff --git a/CITATION.bib b/CITATION.bib index 194d25e4c..14d1a5713 100644 --- a/CITATION.bib +++ b/CITATION.bib @@ -1,6 +1,6 @@ @online{Caesarjl2021, author = {{P}ackage {C}ontributors and {E}cosystem}, - title = {Caesar.jl, v0.10.2}, + title = {Caesar.jl, v0.11.0}, year = {2021}, doi= {Solver DOI: 10.5281/zenodo.5146222}, note = {\url{https://github.com/JuliaRobotics/Caesar.jl}} diff --git a/Project.toml b/Project.toml index 9ee5c8693..c4c66c397 100644 --- a/Project.toml +++ b/Project.toml @@ -2,7 +2,7 @@ name = "Caesar" uuid = "62eebf14-49bc-5f46-9df9-f7b7ef379406" keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics", "ROS"] desc = "Non-Gaussian simultaneous localization and mapping" -version = "0.11.0" +version = "0.11.1" [deps] ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89" @@ -34,6 +34,7 @@ Reexport = "189a3867-3050-52da-a836-e630ba90ab69" Requires = "ae029012-a4dd-5104-9daa-d747884805df" RoME = "91fb55c2-4c03-5a59-ba21-f4ea956187b8" Rotations = "6038ab10-8711-5258-84ad-4b1120ba62dc" +StaticArrays = "90137ffa-7385-5640-81b9-e52037218182" Statistics = "10745b16-79ce-11e8-11f9-7d13ad32a3b2" TensorCast = "02d47bb6-7ce6-556a-be16-bb1710789e2b" TimeZones = "f269a46b-ccf7-5d73-abea-4c690281aa53" @@ -43,47 +44,51 @@ Unmarshal = "cbff2730-442d-58d7-89d1-8e530c41eb02" YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6" [compat] -ApproxManifoldProducts = "0.3, 0.4" +ApproxManifoldProducts = "0.4.21" AprilTags = "0.8, 0.9" Combinatorics = "0.7, 0.8, 0.9, 1" CoordinateTransformations = "0.5, 0.6" DataStructures = "0.16, 0.17, 0.18" -DistributedFactorGraphs = "0.15, 0.16" +DistributedFactorGraphs = "0.15, 0.16, 0.17" Distributions = "0.22, 0.23, 0.24, 0.25" DocStringExtensions = "0.7, 0.8" FFTW = "1" FileIO = "1" ImageCore = "0.7, 0.8, 0.9" ImageMagick = "0.7, 1.0, 1.1" -IncrementalInference = "0.25" +IncrementalInference = "0.25.4, 0.26" JLD2 = "0.3, 0.4" JSON = "0.19, 0.20, 0.21" JSON2 = "0.3, 0.4" KernelDensityEstimate = "0.5" -Manifolds = "0.6.3" +Manifolds = "0.6.3, 0.7" NLsolve = "3, 4" Optim = "1" ProgressMeter = "0.9, 1" Reexport = "0.2, 1" Requires = "0.5, 1" -RoME = "0.16" -Rotations = "0.13, 1" +RoME = "0.16.1, 0.17" +Rotations = "1.1" +StaticArrays = "1" TensorCast = "0.3, 0.4" TimeZones = "1.3.1, 1.4" TransformUtils = "0.2.2" Unmarshal = "0.3, 0.4" YAML = "0.3, 0.4" -ZMQ = "1" julia = "1.4" [extras] AprilTags = "f0fec3d5-a81e-5a6a-8c28-d2b34f3659de" +BSON = "fbb218c0-5317-5bc6-957e-2ee96dd4b1f0" +Colors = "5ae59095-9a9b-59fe-a467-6f913c188581" +Gadfly = "c91e804a-d5a3-530f-b6f0-dfbca275c004" Images = "916415d5-f1e6-5110-898d-aaa5f9f070e0" PyCall = "438e738f-606a-5dbb-bf0a-cddfbfd45ab0" +Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c" RoMEPlotting = "238d586b-a4bf-555c-9891-eda6fc5e55a2" RobotOS = "22415677-39a4-5241-a37a-00beabbbdae8" Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40" ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1" [targets] -test = ["Test", "PyCall", "RobotOS", "ZMQ", "AprilTags", "Images"] +test = ["AprilTags", "BSON", "Colors", "Gadfly", "Images", "PyCall", "Random", "RobotOS", "Test", "ZMQ"] diff --git a/README.md b/README.md index 29850a82e..8db2559a5 100644 --- a/README.md +++ b/README.md @@ -1,10 +1,10 @@

- +

A multimodal/non-Gaussian robotic toolkit for localization and mapping -- reducing the barrier of entry for sensor/data fusion tasks, including Simultaneous Localization and Mapping (SLAM). -[NavAbility.io](http://www.navability.io) helps the with administration and support of the Caesar.jl community, please reach out for any additional information (info@navability.io) or via the Slack badge-link below. +[NavAbility.io](http://www.navability.io) is administrating and supporting the ongoing development of Caesar.jl with and to help grow the community, please reach out for any additional information at info@navability.io or via the Slack badge-link below. # Weblink Info @@ -29,22 +29,24 @@ Code changes are currently tracked via Github's integrated Milestone/Issues/PR s | [ApproxManifoldProducts.jl][amp-url] | [![][amp-stable]][amp-releases] | [![Build Status][amp-build-img]][amp-build-url] | [![codecov.io][amp-cov-img]][amp-cov-url] | [![][mst-shield]][amp-milestones] | | [KernelDensityEstimate.jl][kde-url] | [![][kde-stable]][kde-releases] | [![Build Status][kde-build-img]][kde-build-url] | [![codecov.io][kde-cov-img]][kde-cov-url] | [![][mst-shield]][kde-milestones] | | [FunctionalStateMachine.jl][fsm-url] | [![][fsm-stable]][fsm-releases] | [![Build Status][fsm-build-img]][fsm-build-url] | [![codecov.io][fsm-cov-img]][fsm-cov-url] | [![][mst-shield]][fsm-milestones] | -| [DistributedFactorGraphs.jl][dfg-url] | [![][dfg-stable]][dfg-releases] | [![Build Status][dfg-build-img]][dfg-build-url] | [![codecov.io][dfg-cov-img]][dfg-cov-url] | [![][mst-shield]][dfg-milestones] | -| [LightGraphs.jl][lgraphs-url] | [![][lgjl-stable]][lgjl-releases] | [![Build Status][lgraphs-build-img]][lgraphs-build-url] | [![codecov.io][lgraphs-cov-img]][lgraphs-cov-url] | upstream | -| [Manifolds.jl][mani-url] | [![][mani-stable]][mani-releases] | [![Build Status][mani-build-img]][mani-build-url] | [![codecov.io][mani-cov-img]][mani-cov-url] | upstream | -| [Optim.jl][optim-url] | [![][optim-stable]][optim-releases] | [![Build Status][optim-build-img]][optim-build-url]
[![Build Status][optim-img-windows]][optim-build-windows] | [![codecov.io][optim-cov-img]][optim-cov-url] | upstream | -| [TransformUtils.jl][tf-url] | [![][tf-stable]][tf-releases] | [![Build Status][tf-build-img]][tf-build-url] | [![codecov.io][tf-cov-img]][tf-cov-url] | deprecating | -| [RoMEPlotting.jl][rp-url] | [![][rp-stable]][rp-releases] | [![Build Status][rp-build-img]][rp-build-url] | [![codecov.io][rp-cov-img]][rp-cov-url] | [![][mst-shield]][rp-milestones] | +| [DistributedFactorGraphs.jl][dfg-url] | [![][dfg-ver-img]][dfg-rel-url] | [![Build Status][dfg-ci-dev-img]][dfg-ci-dev-url] | [![codecov.io][dfg-cov-img]][dfg-cov-url] | [![][mst-shield]][dfg-milestones] | +| [LightGraphs.jl][lgraphs-url] | [![][lgjl-stable]][lgjl-releases] | [![Build Status][lgraphs-build-img]][lgraphs-build-url] | [![codecov.io][lgraphs-cov-img]][lgraphs-cov-url] | *upstream* | +| [Manifolds.jl][mani-url] | [![][mani-stable]][mani-releases] | [![Build Status][mani-build-img]][mani-build-url] | [![codecov.io][mani-cov-img]][mani-cov-url] | *upstream* | +| [Optim.jl][optim-url] | [![][optim-stable]][optim-releases] | [![Build Status][optim-build-img]][optim-build-url]
[![Build Status][optim-img-windows]][optim-build-windows] | [![codecov.io][optim-cov-img]][optim-cov-url] | *upstream* | +| [Images.jl][ijl-url] | [![][ijl-ver-stb-img]][ijl-ver-stb-url] | [![Build Status][ijl-action-img]][ijl-action-url] | [![][ijl-codecov-img]][ijl-codecov-url] | *upstream* | +| [AprilTags.jl][apt-url] | [![][apt-ver-img]][apt-ver-url] | [![CI][apt-ci-dev-img]][apt-ci-dev-img] | [![][apt-cov-img]][apt-cov-url] | [![][mst-shield]][apt-milestones] | +| [RoMEPlotting.jl][rp-url] | [![][rp-ver-img]][rp-releases] | [![Build Status][rp-build-img]][rp-build-url] | [![codecov.io][rp-cov-img]][rp-cov-url] | [![][mst-shield]][rp-milestones] | +| [TransformUtils.jl][tf-url] | [![][tf-stable]][tf-releases] | [![Build Status][tf-build-img]][tf-build-url] | [![codecov.io][tf-cov-img]][tf-cov-url] | *deprecating* | # Contributors -We are grateful for many, many contributions within the Julia package ecosystem -- see the [Juliahub.com page](https://juliahub.com/ui/Packages/Caesar/BNbRm/0.10.2?t=1) for the upstream package dependencies. +We are grateful for many, many contributions within the Julia package ecosystem -- see the [Juliahub.com page](https://juliahub.com/ui/Packages/Caesar/BNbRm?page=1) for the upstream package dependencies. Consider citing our work: ``` @online{Caesarjl2021, author = {{P}ackage {C}ontributors and {E}cosystem}, - title = {Caesar.jl, v0.10.2}, + title = {Caesar.jl, v0.11.0}, year = {2021}, doi= {Solver DOI: 10.5281/zenodo.5146222}, note = {\url{https://github.com/JuliaRobotics/Caesar.jl}} @@ -143,20 +145,23 @@ This project adheres to the [JuliaRobotics code of conduct](https://github.com/J [lgraphs-cov-img]: https://codecov.io/github/JuliaGraphs/LightGraphs.jl/coverage.svg?branch=master [lgraphs-cov-url]: https://codecov.io/github/JuliaGraphs/LightGraphs.jl?branch=master -[lgraphs-build-img]: https://travis-ci.org/JuliaGraphs/LightGraphs.jl.svg?branch=master -[lgraphs-build-url]: https://travis-ci.org/JuliaGraphs/LightGraphs.jl +[lgraphs-build-img]: https://github.com/sbromberger/LightGraphs.jl/actions/workflows/ci.yml/badge.svg +[lgraphs-build-url]: https://github.com/sbromberger/LightGraphs.jl/actions/workflows/ci.yml [lgraphs-url]: http://www.github.com/JuliaGraphs/LightGraphs.jl -[lgjl-stable]: https://img.shields.io/badge/2019Q3-v1.3.x-green.svg +[lgjl-stable]: https://juliahub.com/docs/LightGraphs/version.svg +[lgjl-ver-jhub]: https://juliahub.com/ui/Packages/LightGraphs/Xm08G [lgjl-releases]: https://github.com/JuliaGraphs/LightGraphs.jl/releases +[dfg-ci-dev-img]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/actions/workflows/ci.yml/badge.svg +[dfg-ci-dev-url]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/actions/workflows/ci.yml +[dfg-ci-stb-img]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/actions/workflows/ci.yml/badge.svg?branch=release%2Fv0.16 +[dfg-ci-stb-url]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/actions/workflows/ci.yml +[dfg-ver-img]: https://juliahub.com/docs/IncrementalInference/version.svg [dfg-cov-img]: https://codecov.io/github/JuliaRobotics/DistributedFactorGraphs.jl/coverage.svg?branch=master [dfg-cov-url]: https://codecov.io/github/JuliaRobotics/DistributedFactorGraphs.jl?branch=master -[dfg-build-img]: https://travis-ci.org/JuliaRobotics/DistributedFactorGraphs.jl.svg?branch=master -[dfg-build-url]: https://travis-ci.org/JuliaRobotics/DistributedFactorGraphs.jl [dfg-url]: http://www.github.com/JuliaRobotics/DistributedFactorGraphs.jl -[dfg-stable]: https://img.shields.io/badge/2021Q2-v0.14.x-green.svg [dfg-milestones]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/milestones -[dfg-releases]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/releases +[dfg-rel-url]: https://github.com/JuliaRobotics/DistributedFactorGraphs.jl/releases [amp-cov-img]: https://codecov.io/github/JuliaRobotics/ApproxManifoldProducts.jl/coverage.svg?branch=master [amp-cov-url]: https://codecov.io/github/JuliaRobotics/ApproxManifoldProducts.jl?branch=master @@ -190,8 +195,28 @@ This project adheres to the [JuliaRobotics code of conduct](https://github.com/J [rp-url]: http://www.github.com/JuliaRobotics/RoMEPlotting.jl [rp-cov-img]: https://codecov.io/github/JuliaRobotics/RoMEPlotting.jl/coverage.svg?branch=master [rp-cov-url]: https://codecov.io/github/JuliaRobotics/RoMEPlotting.jl?branch=master -[rp-build-img]: https://travis-ci.org/JuliaRobotics/RoMEPlotting.jl.svg?branch=master -[rp-build-url]: https://travis-ci.org/JuliaRobotics/RoMEPlotting.jl -[rp-stable]: https://img.shields.io/badge/2021Q2-v0.7.x-green.svg +[rp-build-img]: https://github.com/JuliaRobotics/RoMEPlotting.jl/actions/workflows/ci.yml/badge.svg +[rp-build-url]: https://github.com/JuliaRobotics/RoMEPlotting.jl/actions/workflows/ci.yml +[rp-ver-img]: https://juliahub.com/docs/RoMEPlotting/version.svg [rp-milestones]: https://github.com/JuliaRobotics/RoMEPlotting.jl/milestones [rp-releases]: https://github.com/JuliaRobotics/RoMEPlotting.jl/releases + +[ijl-url]: https://github.com/JuliaImages/Images.jl +[ijl-pkgeval-img]: https://juliaci.github.io/NanosoldierReports/pkgeval_badges/I/Images.svg +[ijl-pkgeval-url]: https://juliaci.github.io/NanosoldierReports/pkgeval_badges/report.html +[ijl-action-img]: https://github.com/JuliaImages/Images.jl/workflows/Unit%20test/badge.svg +[ijl-action-url]: https://github.com/JuliaImages/Images.jl/actions +[ijl-codecov-img]: https://codecov.io/github/JuliaImages/Images.jl/coverage.svg?branch=master +[ijl-codecov-url]: https://codecov.io/github/JuliaImages/Images.jl?branch=master +[ijl-ver-stb-img]: https://juliahub.com/docs/Images/version.svg +[ijl-ver-stb-url]: https://github.com/JuliaImages/Images.jl/releases + + +[apt-url]: https://github.com/JuliaRobotics/AprilTags.jl +[apt-ver-img]: https://juliahub.com/docs/AprilTags/version.svg +[apt-ver-url]: https://github.com/JuliaRobotics/AprilTags.jl/releases +[apt-ci-dev-img]: https://travis-ci.org/JuliaRobotics/AprilTags.jl.svg?branch=master +[apt-ci-dev-url]: https://travis-ci.org/JuliaRobotics/AprilTags.jl +[apt-cov-img]: http://codecov.io/github/JuliaRobotics/AprilTags.jl/coverage.svg?branch=master +[apt-cov-url]: http://codecov.io/github/JuliaRobotics/AprilTags.jl?branch=master +[apt-milestones]: https://github.com/JuliaRobotics/AprilTags.jl/milestones diff --git a/docs/make.jl b/docs/make.jl index 509053bfb..86e33bde7 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -20,50 +20,68 @@ makedocs( format = Documenter.HTML(), sitename = "Caesar.jl", pages = Any[ - "Introduction" => "index.md", - "Welcome" => [ + "Welcome" => "index.md", + "Introduction" => [ + "Introduction" => "introduction.md", + "Gaussian vs. Non-Gaussian" => "concepts/why_nongaussian.md", "Installation" => "installation_environment.md", + "Using Julia" => "concepts/using_julia.md", "FAQ" => "faq.md", ], "Getting Started" => [ "Initial Concepts" => "concepts/concepts.md", - "Building Factor Graphs" => "concepts/building_graphs.md", - "Solving and Interacting" => "concepts/interacting_fgs.md", - "Internal Variables/Factors" => "concepts/available_varfacs.md", + "Building Graphs" => "concepts/building_graphs.md", + "Solving Graphs" => "concepts/solving_graphs.md", + "Interact w Graphs" => "concepts/interacting_fgs.md", "Multi-Modal/Hypothesis" => "concepts/dataassociation.md", - "Flux (NN) Factors" => "concepts/flux_factors.md", - "Plotting (2D)" => "concepts/2d_plotting.md", - "Entry=>Data Blob" => "concepts/entry_data.md", - "Multi-Language Support" => "concepts/multilang.md", - "Cloud Server/Database" => "concepts/database_interactions.md", - "Multi-session/agent Solving" => "concepts/multisession.md", - "Parametric Solve (EXP)" => "examples/parametric_solve.md", - "Visualization (3D)" => "concepts/arena_visualizations.md", - ], + "Parallel Processing" => "concepts/parallel_processing.md", + "[DEV] Parametric Solve" => "examples/parametric_solve.md", + ], "Examples" => [ "Caesar Examples" => "examples/examples.md", - "ContinuousScalar as 1D Example" => "examples/basic_continuousscalar.md", - "Under-defined Trilateration, 2D" => "examples/basic_slamedonut.md", + "Canonical 1D Example" => "examples/basic_continuousscalar.md", + "Underconstrained Range-only" => "examples/basic_slamedonut.md", "Hexagonal 2D SLAM" => "examples/basic_hexagonal2d.md", "Fixed-Lag Solving 2D" => "examples/interm_fixedlag_hexagonal.md", - "ROS Middleware" => "examples/using_ros.md", "Dead Reckon Tether" => "examples/deadreckontether.md", ], + "Graph Library" => [ + "Canonical Generators" => "examples/canonical_graphs.md", + "Entry=>Data Blob" => "concepts/entry_data.md", + "Variables/Factors" => "concepts/available_varfacs.md", + "Flux (NN) Factors" => "concepts/flux_factors.md", + "Images and AprilTags" => "examples/using_images.md", + ], + "Visualization" => [ + "Installing Viz" => "install_viz.md", + "Plotting (2D)" => "concepts/2d_plotting.md", + "Visualization (3D)" => "concepts/arena_visualizations.md", + ], + "Middlewares" => [ + "ROS Middleware" => "examples/using_ros.md", + "Compile Binaries" => "concepts/compile_binary.md", + "Zero Install Solution" => "concepts/zero_install.md", + "Multi-session/agent Solving" => "concepts/multisession.md", + "Multi-Language Support" => "concepts/multilang.md", + ], + "How to Expand?" => [ + "Pkg Framework" => "caesar_framework.md", + "Custom Variables and Factors" => "examples/adding_variables_factors.md", + "Creating Variables" => "examples/custom_variables.md", + "Creating Factors" => "examples/basic_definingfactors.md", + "More Functions" => "func_ref.md", + ], "Principles" => [ "Filters vs. Graphs" => "principles/filterCorrespondence.md", "Generic Convolutions" => "principles/approxConvDensities.md", "Multiplying Functions (.py)" => "principles/multiplyingDensities.md", "Bayes (Junction) tree" => "principles/bayestreePrinciples.md", "Advanced Bayes Tree Topics" => "principles/initializingOnBayesTree.md", - "Multimodal iSAM Algorithm" => "concepts/mmisam_alg.md", - ], - "How to Expand?" => [ - "Custom Variables and Factors" => "examples/adding_variables_factors.md", - "Creating Variables" => "examples/custom_variables.md", - "Creating Factors" => "examples/basic_definingfactors.md", + "Non-Gaussian Algorithm" => "concepts/mmisam_alg.md", ], "Developer Zone" => [ "Wiki Pointers" => "dev/wiki.md", + "Legacy Factors" => "examples/legacy_deffactors.md", "Creating DynPose Factor" => "principles/interm_dynpose.md", "Known Issue List" => "dev/known_issues.md", "Internal Functions" => "dev/internal_fncs.md", @@ -71,9 +89,6 @@ makedocs( "Literature" => [ "References" => "refs/literature.md" ], - "Function Reference" => [ - "Caesar's Reference" => "func_ref.md" - ] ] # html_prettyurls = !("local" in ARGS), ) diff --git a/docs/src/caesar_framework.md b/docs/src/caesar_framework.md new file mode 100644 index 000000000..dc48349e9 --- /dev/null +++ b/docs/src/caesar_framework.md @@ -0,0 +1,30 @@ + +# The Caesar Framework + +The [Caesar.jl](https://github.com/JuliaRobotics/Caesar.jl) package is an "umbrella" framework around other dedicated algorithmic packages. While most of the packages are implemented in native [Julia](http://www.julialang.org/) ([JuliaPro](http://www.juliacomputing.com)), a few dependencies are wrapped C libraries. Note that C/C++ [can be incorporated with zero overhead](https://docs.julialang.org/en/v1/manual/calling-c-and-fortran-code/), such as was done with [AprilTags.jl](http://www.github.com/JuliaRobotics/AprilTags.jl). + +> [FAQ: Why use Julia?](https://www.juliarobotics.org/Caesar.jl/latest/faq/#Why-Julia-1) + +## AMP / IIF / RoME + +Robot motion estimate ([RoME.jl](http://www.github.com/JuliaRobotics/RoME.jl)) can operate in the conventional SLAM manner, using local memory (dictionaries), or alternatively distribute over a persisted [`DistributedFactorGraph.jl`](http://www.github.com/JuliaRobotics/DistributedFactorGraphs.jl) through common serialization and graph storage/database technologies, [see this article as example](http://people.csail.mit.edu/spillai/projects/cloud-graphs/2017-icra-cloudgraphs.pdf) [1.3]. +A variety of 2D plotting, 3D visualization, serialization, middleware, and analysis tools come standard as provided by the associated packages. RoME.jl combines reference frame transformations and robotics SLAM tool around the back-end solver provides by [IncrementalInference.jl](http://www.github.com/JuliaRobotics/IncrementalInference.jl). + +Details about the accompanying packages: +* [IncrementalInference.jl](http://www.github.com/JuliaRobotics/IncrementalInference.jl) supplies the algebraic logic for factor graph inference with Bayes tree and depends on several packages itself. +* [RoME.jl](http://www.github.com/JuliaRobotics/RoME.jl) introduces nodes and factors that are useful to robotic navigation. +* [ApproxManifoldProducts.jl](http://www.github.com/JuliaRobotics/ApproxManifoldProducts.jl) provides on-manifold belief product operations. + +## Visualization (Arena.jl/RoMEPlotting.jl) +Caesar visualization (plotting of results, graphs, and data) is provided by 2D and 3D packages respectively: +* [RoMEPlotting.jl](http://www.github.com/JuliaRobotics/RoMEPlotting.jl) are a set of scripts that provide MATLAB style plotting of factor graph beliefs, mostly supporting 2D visualization with some support for projections of 3D; +* [Arena.jl](https://github.com/dehann/Arena.jl) package, which is a collection of 3D visualization tools. + +## Multilanguage Interops: NavAbility.io SDKs and APIs + +The Caesar framework is not limited to direct Julia use. Check out www.NavAbility.io, or contact directly at (info@navabiliyt.io), for more details. Also see the [community multi-language page](https://www.juliarobotics.org/Caesar.jl/latest/concepts/multilang/) for details. + + +!!! note + [FAQ: Interop with other languages (not limited to Julia only)](https://www.juliarobotics.org/Caesar.jl/latest/faq/#Is-Caesar.jl-limited-to-Julia?-No.-1) + diff --git a/docs/src/concepts/2d_plotting.md b/docs/src/concepts/2d_plotting.md index e766e2f75..aa1ffeea4 100644 --- a/docs/src/concepts/2d_plotting.md +++ b/docs/src/concepts/2d_plotting.md @@ -1,4 +1,4 @@ -# Plotting +# Plotting 2D Once the graph has been built, 2D plot visualizations are provided by [RoMEPlotting.jl](http://www.github.com/JuliaRobotics/RoMEPlotting.jl) and [KernelDensityEstimatePlotting.jl](http://www.github.com/JuliaRobotics/KernelDensityEstimatePlotting.jl). These visualizations tools are readily modifiable to highlight various aspects of mobile platform navigation. diff --git a/docs/src/concepts/available_varfacs.md b/docs/src/concepts/available_varfacs.md index d6e0b217f..37cc8cb21 100644 --- a/docs/src/concepts/available_varfacs.md +++ b/docs/src/concepts/available_varfacs.md @@ -1,4 +1,4 @@ -# Variables in Caesar.jl +# [Variables in Caesar.jl](@id variables_factors) You can check for the latest variable types by running the following in your terminal: @@ -8,8 +8,10 @@ subtypes(IncrementalInference.InferenceVariable) IncrementalInference.getCurrentWorkspaceVariables() ``` -!!! note - More variables and factors exists, but are not yet included in the standard library. It is fairly straight forward to build your own out-of-library factors, see page [Creating New Variables and Factors](@ref) for more details. + +The variables and factors in Caesar should be sufficient for a variety of robotic applications, however, users can easily extend the framework (without changing the core code). This can even be done *out-of-library* at runtime after a construction of a factor graph has started! See [Custom Variables](@ref custom_variables) and [Custom Factors](@ref custom_factors) for more details. + +## Basic Variables Default variables in IncrementalInference diff --git a/docs/src/concepts/building_graphs.md b/docs/src/concepts/building_graphs.md index 3926c1982..502bae4cc 100644 --- a/docs/src/concepts/building_graphs.md +++ b/docs/src/concepts/building_graphs.md @@ -1,30 +1,25 @@ -# Building and Solving Graphs +# [Building Graphs](@ref building_graphs) Irrespective of your application - real-time robotics, batch processing of survey data, or really complex multi-hypothesis modeling - you're going to need to add factors and variables to a graph. This section discusses how to do that in Caesar. The following sections discuss the steps required to construct a graph and solve it: -* Initialing the Factor Graph +* Initializing the Factor Graph * Adding Variables and Factors to the Graph * Solving the Graph * Informing the Solver About Ready Data ## Familiar Canonical Factor Graphs -Starting with a shortcut to just quickly getting a small predefined *canonical* graph containing a few variables and factors can be done with (try tab-completion in the REPL): -```julia -fg = generateCanonicalFG_Kaess() -fg = generateCanonicalFG_LineStep() -fg = generateCanonicalFG_Hexagonal() -fg = generateCanonicalFG_Circle() -``` - -Any one of these function *generate* a standard factor graph object that is useful for orientation, testing, learning, or validation. You can generate any of these factor graphs at any time, for example when quickly wanting to test some idea midway through building a more sophisiticated `fg`, you might just want to quickly do: +Starting with a shortcut to just quickly getting a small predefined *canonical* graph containing a few variables and factors. Functions to *generate* a canonical factor graph object that is useful for orientation, testing, learning, or validation. You can generate any of these factor graphs at any time, for example when quickly wanting to test some idea midway through building a more sophisiticated `fg`, you might just want to quickly do: ```julia fg_ = generateCanonicalFG_Hexagonal() ``` and then work with `fg_` to try out something risky. +!!! note + See the [Canonical Graphs](../examples/canonical_graphs.md) page for a more complete list of existing graph generators. + ## Building a new Graph The first step is to model the data (using the most appropriate *factors*) among *variables* of interest. To start model, first create a *distributed factor graph object*: @@ -47,7 +42,7 @@ Variables (a.k.a. poses or states in navigation lingo) are created with the `add addVariable!(fg, :x0, Pose2) # Add a few more poses for i in 1:10 - addVariable!(fg, Symbol("x$(i)"), Pose2) + addVariable!(fg, Symbol("x",i), Pose2) end ``` @@ -143,8 +138,8 @@ Computation will progress faster if poses and landmarks are very sparse. To ext For completeness, one could also re-project the most meaningful measurements from sensor measurements between pose epochs as though measured from the pose epoch. This approach essentially marginalizes the local dead reckoning drift errors into the local interpose re-projections, but helps keep the pose count low. -In addition, see [fixed-lag discussion](https://www.juliarobotics.org/Caesar.jl/latest/examples/examples/#Hexagonal-2D-1) for limiting during inference the number of fluid variables manually to a user desired count. +In addition, see [Fixed-lag Solving](@ref fixedlag_solving) for limiting during inference the number of fluid variables manually to a user desired count. ## Which Variables and Factors to use -See the next page on [available variables and factors](https://www.juliarobotics.org/Caesar.jl/latest/concepts/available_varfacs/) +See the next page on [available variables and factors](@ref variables_factors) diff --git a/docs/src/concepts/compile_binary.md b/docs/src/concepts/compile_binary.md new file mode 100644 index 000000000..ed5570cdf --- /dev/null +++ b/docs/src/concepts/compile_binary.md @@ -0,0 +1,22 @@ +# [Compile Binaries](@id compile_binaries) + +Broader Julia ecosystem work on compiling shared libraries and images is hosted by [PackageCompiler.jl](https://github.com/JuliaLang/PackageCompiler.jl), see documentation there. + +## Compiling RoME.so + +A default RoME system image script can be used [`compileRoME/compileRoMESysimage.jl`](https://github.com/JuliaRobotics/RoME.jl/blob/master/compileRoME/compileRoMESysimage.jl) to reduce the "time-to-first-plot". + +To use RoME with the newly created sysimage, start julia with: +``` +julia -O3 -J ~/.julia/dev/RoME/compileRoME/RoMESysimage.so +``` + +Which should dramatically cut down on the load time of the included package JIT compilation. More packages or functions can be added to the binary, depending on the application. Furthermore, full executable binaries can easily be made with PackageCompiler.jl. + +## More Info + +!!! note + Also see [this Julia Binaries Blog](https://medium.com/@sdanisch/compiling-julia-binaries-ddd6d4e0caf4). [More on discourse.](https://discourse.julialang.org/t/ann-packagecompiler-with-incremental-system-images/20489). Also see new brute force sysimg work at [Fezzik.jl](https://github.com/TsurHerman/Fezzik). + +!!! note + Contents of a previous blog post [this AOT vs JIT compiling blog post](https://juliacomputing.com/blog/2016/02/09/static-julia.html) has been wrapped into PackageCompiler.jl. diff --git a/docs/src/concepts/concepts.md b/docs/src/concepts/concepts.md index 965df46e5..a76da36e6 100644 --- a/docs/src/concepts/concepts.md +++ b/docs/src/concepts/concepts.md @@ -1,22 +1,16 @@ -# Caesar Concepts +# Graph Concepts -A factor graph is a bipartite representation where variables (denoted by larger nodes) are interconnected by a set of factors (smaller nodes) that represent some algebraic interaction between the variables. Factors must adhere to the limits of probabilistic models -- for example conditional likelihoods (between multiple variables) or priors (unary to one variable). A more heterogeneous factor graph example is shown below; see a broader discussion [in the related literature ](https://juliarobotics.org/Caesar.jl/latest/refs/literature/): +Factor graphs are bipartite consisting of *variables* and *factors*, which are connected by edges to form a graph structure. The terminology of nodes is reserved for actually storing the data on some graph oriented technology. -![factorgraphexample](https://user-images.githubusercontent.com/6412556/41196136-e5b05f98-6c07-11e8-8f26-7318e5085cc0.png) - -# Why/Where does non-Gaussian data come from? +## What are Variables and Factors -Gaussian error models in measurement or data cues will only be Gaussian (normally distributed) if all physics/decisions/systematic-errors/calibration/etc. has a correct algebraic model in all circumstances. Caesar.jl and MM-iSAMv2 is heavily focussed on state-estimation from a plethora of heterogenous data that may not yet have perfect algebraic models. Four major categories of non-Gaussian errors have thus far been considered: -- Uncertain decisions (a.k.a. data association), such as a robot trying to decide if a navigation loop-closure can be deduced from a repeat observation of a similar object or measurement from current and past data. These issues are commonly also referred to as multi-hypothesis. -- Underdetermined or underdefined systems where there are more variables than constraining measurements to fully define the system as a single mode---a.k.a solution ambiguity. For example, in 2D consider two range measurements resulting in two possible locations through trilateration. -- Nonlinearity. For example in 2D, consider a Pose2 odometry where the orientation is uncertain: The resulting belief of where a next pose might be (convolution with odometry factor) results in a banana shape curve, even though the entire process is driven by assumed Gaussian belief. -- Physics of the measurement process. Many measurement processes exhibit non-Gaussian behaviour. For example, acoustic/radio time-of-flight measurements, using either pulse-train or matched filtering, result in an "energy intensity" over time/distance of what the range to a scattering-target/source might be--i.e. highly non-Gaussian. +Variables, denoted as the larger nodes in the figur below, represent state variables of interest such as vehicle or landmark positions, sensor calibration parameters, and more. Variables are likely hidden values which are not directly observed, but we want to estimate them them from observed data and at least some minimal algebra structure from probabilistic measurement models. -## What are Variables and Factors +Factors, the smaller nodes in the figure, represent the algebraic interaction between particular variables, which is captured through edges. Factors must adhere to the limits of probabilistic models -- for example conditional likelihoods capture the likelihood correlations between variables; while priors (unary to one variable) represent absolute information to be introduced. A heterogeneous factor graph illustration is shown below; also see a broader discussion [linked on the literature page](https://juliarobotics.org/Caesar.jl/latest/refs/literature/). -Factor graphs are bipartite, i.e. variables and factors. The terminology of nodes and edges is reserved for actually storing the data on some graph-based technology. +![factorgraphexample](https://user-images.githubusercontent.com/6412556/41196136-e5b05f98-6c07-11e8-8f26-7318e5085cc0.png) -Variables in the factor graph have not been observed, but we want to back them out given the observed values and algebra defining the structure between all observations. Mathematically speaking, factors are actually "observed variables" that are stochastically "fixed". Waving hands over the fact that factors encode both the algebraic model AND the observed measurement values. If factors are constructed from statistically independent measurements (i.e. no direct correlations between measurements other than the known algebraic model that might connect them), then we can use Probabilistic Chain rule to write inference operation down (unnormalized): +We assume factors are constructed from statistically independent measurements (i.e. no direct correlations between measurements other than the known algebraic model that might connect them), then we can use Probabilistic Chain rule to write inference operation down (unnormalized): ```math P(\Theta | Z) \propto P(Z | \Theta) P(\Theta) @@ -34,109 +28,12 @@ or similarly, P(\Theta, Z) = P(\Theta | Z) P(Z). ``` -Second, the uncorrelated measurement process assumption implies that `` P(Z) `` constant given the algebraic model. - -# Getting Started with Caesar - -This section discusses the various concepts in the Caesar framework. - -## Julia and Help - -When launching the REPL in a terminal or and IDE like VS Code (see link for documtation website): -```bash -$ julia -O3 - _ - _ _ _(_)_ | Documentation: https://docs.julialang.org - (_) | (_) (_) | - _ _ _| |_ __ _ | Type "?" for help, "]?" for Pkg help. - | | | | | | |/ _` | | - | | |_| | | | (_| | | Version 1.5.2 (2020-09-23) - _/ |\__'_|_|_|\__'_| | Official https://julialang.org/ release -|__/ | -``` - -The `-O 3` is for level 3 code compilation optimization and is a useful habit for slightly faster execution, but slightly slower first run just-in-time compilation of any new function. - -To get help with a function, just start with the `?` character followed by the function name, e.g.: -```julia -?sin -# help?> sin -search: sin sinh sind sinc sinpi sincos sincosd SingleThreaded SingularException asin using isinf asinh asind isinteger isinteractive - - sin(x) - - Compute sine of x, where x is in radians. - - ───────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────────── - - sin(A::AbstractMatrix) - - Compute the matrix... -``` - -## Loading Packages - -Assuming you just loaded an empty REPL, or at the start of a script, or working inside the VSCode IDE, the first thing to do is load the necessary Julia packages. Caesar.jl is an umbrella package potentially covering over 100 Julia Packages. For this reason the particular parts of the code are broken up amongst more focussed *vertical purpose* library packages. Usually for Robotics either `Caesar` or less expansive `RoME` will do. Other non-Geometric sensor processing applications might build in the MM-iSAMv2, Bayes tree, and DistributedFactorGraph libraries. Any of these packages can be loaded as follows: - -```julia -# umbrella containing most functional packages including RoME -using Caesar -# contains the IncrementalInference and other geometric manifold packages -using RoME -# contains among others DistributedFactorGraphs.jl and ApproxManifoldProducts.jl -using IncrementalInference -``` - -### Requires.jl for Optional Package Loading - -Many of these packages have additional features that are not included by default. For example, the [Flux.jl](https://fluxml.ai/Flux.jl/stable/) machine learning package will introduce several additional features when loaded, e.g.: -```julia -julia> using Flux, RoME - -[ Info: IncrementalInference is adding Flux related functionality. -[ Info: RoME is adding Flux related functionality. -``` - -For completeness, so too with packages like `Images.jl`, `RobotOS.jl`, and others: -```julia -using Caesar, Images +The inference objective is to invert this system, so as to find the states given the product between all the likelihood models (based on the data): +```math +P(\Theta | Z) \propto \prod_i P(Z_i | \Theta_i) \prod_j P(\Theta_j) ``` -## Parallel Processing +We use the uncorrelated measurement process assumption that measurements Z are independent given the constructed algebraic model. !!! note - Keywords: parallel processing, multi-threading, multi-process - -Julia allows [high-performance, parallel processing from the ground up](https://docs.julialang.org/en/v1/manual/parallel-computing/). Depending on the configuration, Caesar.jl can utilize a combination of four styles of multiprocessing: i) separate memory multi-process; ii) shared memory multi-threading; iii) asynchronous shared-memory (forced-atomic) co-routines; and iv) multi-architecture such as JuliaGPU. As of Julia 1.4, the most reliable method of loading all code into all contexts (for multi-processor speedup) is as follows. - -### Multithreading and Multiprocessing - -Make sure the environment variable `JULIA_NUM_THREADS` is set as default or per call and recommended to use 4 as starting point. -``` -JULIA_NUM_THREADS=4 julia -O3 -``` - -In addition to multithreading, Caesar.jl utilizes multiprocessing to distribute computation during the inference steps. Following standard Julia, more processes can be added as follows: -```julia -# load the required packages into procid()==1 -using Flux, RoME, Caesar, RoMEPlotting - -# then start more processes -using Distributed -addprocs(8) # note this yields 6*8=40 possible processing threads - -# now make sure all code is loaded everywhere (for separate memory cases) -@everywhere using Flux, RoME, Caesar -``` - -It might also be convenient to warm up some of the Just-In-Time compiling: -```julia -# solve a few graphs etc, to get majority of solve code compiled before running a robot. -[warmUpSolverJIT() for i in 1:3]; -``` - -The best way to avoid compile time (when not developing) is to use the established Julia "first time to plot" approach based on PackageCompiler.jl, and more details are provided at [Ahead of Time compiling](https://juliarobotics.org/Caesar.jl/latest/installation_environment/#Ahead-Of-Time-Compile-RoME.so), and a few common questions might be answered via [FAQ here](https://juliarobotics.org/Caesar.jl/latest/faq/#Static,-Shared-Object-.so-Compilation). - -The next section describes the initial steps in constructing and solving graphs will be discussed in the upcoming documentation page [Building and Solving Graphs](building_graphs.md). We also recommend reviewing the various examples available in the [Examples section](../examples/examples.md). The variables and factors in Caesar should be sufficient for the majority of robotic applications, however Caesar allows users to extend the framework without changing the core code. This is discussed in [Creating New Variables and Factors](../examples/adding_variables_factors.md). Caesar supports both in-memory solving (fast, for moderately-sized graphs) as well as [shared data persistence and inference](database_interactions.md) for massive graphs, multiple sessions, and multiple agents. - -Although Caesar is Julia-based, it provides multi-language support with a ZMQ interface. This is discussed in [Caesar Multi-Language Support](multilang.md). Caesar.jl also supports various visualizations and plots by using Arena, RoMEPlotting, and Director. This is discussed in [Visualization with Arena.jl and RoMEPlotting.jl](arena_visualizations.md). + Strictly speaking, factors are actually "observed variables" that are stochastically "fixed" and not free for estimation in the conventional SLAM perspective. Waving hands over the fact that factors encode both the algebraic model and the observed measurement values provides a perspective on learning structure of a problem, including more mundane operations such as sensor calibration or learning of channel transfer models. \ No newline at end of file diff --git a/docs/src/concepts/database_interactions.md b/docs/src/concepts/database_interactions.md deleted file mode 100644 index 209bdbd23..000000000 --- a/docs/src/concepts/database_interactions.md +++ /dev/null @@ -1,9 +0,0 @@ -## Using The Cloud Server - -See ongoing work at: -- [DistributedFactorGraphs.jl](https://github.com/JuliaRobotics/DistributedFactorGraphs.jl), -- [GraffSDK.jl](https://github.com/GearsAD/GraffSDK.jl). - -### Cloud Server offers Multi-session/agent Support - -Next [page Multisession](https://www.juliarobotics.org/Caesar.jl/latest/concepts/multisession/) diff --git a/docs/src/concepts/interacting_fgs.md b/docs/src/concepts/interacting_fgs.md index dec6a3e39..61decfd44 100644 --- a/docs/src/concepts/interacting_fgs.md +++ b/docs/src/concepts/interacting_fgs.md @@ -93,37 +93,6 @@ Sometimes it is useful to make a deepcopy of a segment of the factor graph for s sfg = buildSubgraph(fg, [:x1;:x2;:l7], 1) ``` -# Solving Graphs - -When you have built the graph, you can call the solver to perform inference with the following: - -```julia -# Perform inference -tree = solveTree!(fg) -``` - -The returned Bayes (Junction) `tree` object is described in more detail on [a dedicated documentation page](https://juliarobotics.org/Caesar.jl/latest/principles/bayestreePrinciples/), while `smt` and `hist` return values most closely relate to development and debug outputs which can be ignored during general use. Should an error occur during, the exception information is easily accessible in the `smt` object (as well as file logs which default to `/tmp/caesar/`). - -```@docs -solveTree! -``` - -## Using Incremental Updates (Clique Recycling I) - -One of the major features of the MM-iSAMv2 algorithm (implemented by [IncrementalInference.jl](http://www.github.com/JuliaRobotics/IncrementalInference.jl)) is reducing computational load by recycling and marginalizing different (usually older) parts of the factor graph. In order to utilize the benefits of recycing, the previous Bayes (Junction) tree should also be provided as input (see fixed-lag examples for more details): -```julia -tree = solveTree!(fg, tree) -``` - -## Using Clique out-marginalization (Clique Recycling II) - -When building sysmtes with limited computation resources, the out-marginalization of cliques on the Bayes tree can be used. This approach limits the amount of variables that are inferred on each solution of the graph. This method is also a compliment to the above Incremental Recycling -- these two methods can work in tandem. There is a default setting for a FIFO out-marginalization strategy (with some additional tricks): -```julia -defaultFixedLagOnTree!(fg, 50, limitfixeddown=true) -``` - -This call will keep the latest 50 variables fluid for inference during Bayes tree inference. The keyword `limitfixeddown=true` in this case will also prevent downward message passing on the Bayes tree from propagating into the out-marginalized branches on the tree. A later page in this documentation will discuss how the inference algorithm and Bayes tree aspects are put together. - # Extracting Belief Results (and PPE) Once you have solved the graph, you can review the full marginal with: diff --git a/docs/src/concepts/multilang.md b/docs/src/concepts/multilang.md index 7b512b7e3..e83b05a3f 100644 --- a/docs/src/concepts/multilang.md +++ b/docs/src/concepts/multilang.md @@ -1,10 +1,27 @@ -# Multi-Language and Shared Objects +# Multilanguage Interops -## Multilanguage Interops: Caesar SDKs and APIs -The Caesar framework is not limited to direct Julia use. The following Github projects provide access to features of Caesar in their language: +The Caesar framework is not limited to direct Julia use. + +## Static, Shared Object `.so` Compilation +See [Compiling Binaries](@ref compile_binaries). + +## ROS Integration +See [ROS Integration](@ref ros_direct). + +## Python Direct + +For completeness, another design pattern is to wrap Julia packages for direct access from python, see [SciML/diffeqpy](https://github.com/SciML/diffeqpy) as example. + +## Caesar SDKs and APIs + +The maintainers of Caesar.jl together with NavAbility.io are developing a standardized SDK / API for much easier multi-language / multi-access use of the solver features. Contact info@navability.io for more information. !!! note - 21Q1, a new multilanguage interface is under development and will replace and consolidate the previous methods listed below. + 2021Q4, Coming Soon! A new multilanguage SDK is under development and will replace and consolidate the previous methods listed below. + +### Previous Generation APIs + +The following Github projects provide access to features of Caesar in their language: * Julia Web interface: * [GraffSDK.jl](https://github.com/GearsAD/GraffSDK.jl) @@ -13,19 +30,14 @@ The Caesar framework is not limited to direct Julia use. The following Github pr * C/C++: * [Graff Cpp](https://github.com/MarineRoboticsGroup/graff_cpp) * [Caesar LCM](http://github.com/pvazteixeira/caesar-lcm) + * [Caesar ROS](http://github.com/pvazteixeira/caesar_ros) * Python: - * [GraffSDK.py](https://github.com/nicrip/graff_py) + * [GraffSDK.py](https://github.com/nicrip/graff_py) (needs to be updated) * [Synchrony_py](https://github.com/nicrip/SynchronySDK_py) -Please contact info@navability.io for more information. - ## ZMQ Messaging Interface -Caesar.jl has a ZMQ messaging interface ([interested can see code here](https://github.com/JuliaRobotics/Caesar.jl/blob/master/src/zmq/ZmqCaesar.jl)) that allows users to interact with the solver code base in a variety of ways. The messaging interface is not meant to replace static `.so` library file compilation--see below---but rather provide a more versatile and flexible development strategy. - -The current known interface implementations to Caesar.jl are: -- C/C++ [GraffCPP](https://github.com/MarineRoboticsGroup/graff_cpp), -- Python [GraffSDK.py](https://github.com/nicrip/graff_py) (needs to be updated), +Caesar.jl has a ZMQ messaging interface ([interested can see code here](https://github.com/JuliaRobotics/Caesar.jl/blob/master/src/zmq/ZmqCaesar.jl)) that allows users to interact with the solver code base in a variety of ways. The messaging interface is not meant to replace static `.so` library file compilation but rather provide a more versatile and flexible development strategy. ### Starting the Caesar ZMQ Navigation Server @@ -53,21 +65,6 @@ start(zmqConfig) The [current tests are a good place to see some examples](http://github.com/JuliaRobotics/Caesar.jl/tree/master/test/multilangzmq) of the current interfacing functions. Feel free to change the ZMQ interface for to any of the ZMQ supported modes of data transport, such as [Interprocess Communication (IPC)](http://api.zeromq.org/2-1:zmq-ipc) vs. TCP. -> TODO: expand the ZMQ documentation - -### ROS Integration - -Yes, but not yet. See: - -> [FAQ: ROS Integration](http://www.juliarobotics.org/Caesar.jl/latest/faq/#ROS-Integration-1) - -## Static, Shared Object `.so` Compilation - -> [FAQ: Static, Shared Object `.so` Compilation](.http://www.juliarobotics.org/Caesar.jl/latest/faq/#Static,-Shared-Object-.so-Compilation-1) - -The plan for the `Caesar.jl` & the mm-iSAM is to use [PackageCompiler.jl](https://github.com/JuliaLang/PackageCompiler.jl) to generate linkable `.o` or `.so` files; or maybe even full executables. - -> Please add your comments to [this issue discussion](https://github.com/JuliaRobotics/Caesar.jl/issues/210). ## Alternative Methods diff --git a/docs/src/concepts/parallel_processing.md b/docs/src/concepts/parallel_processing.md new file mode 100644 index 000000000..f1509c897 --- /dev/null +++ b/docs/src/concepts/parallel_processing.md @@ -0,0 +1,72 @@ +# Parallel Processing + +!!! note + Keywords: parallel processing, multi-threading, multi-process + +Julia allows [high-performance, parallel processing from the ground up](https://docs.julialang.org/en/v1/manual/parallel-computing/). Depending on the configuration, Caesar.jl can utilize a combination of four styles of multiprocessing: i) separate memory multi-process; ii) shared memory multi-threading; iii) asynchronous shared-memory (forced-atomic) co-routines; and iv) multi-architecture such as JuliaGPU. As of Julia 1.4, the most reliable method of loading all code into all contexts (for multi-processor speedup) is as follows. + +## Multiprocessing + +Make sure the environment variable `JULIA_NUM_THREADS` is set as default or per call and recommended to use 4 as starting point. +``` +JULIA_NUM_THREADS=4 julia -O3 +``` + +In addition to multithreading, Caesar.jl utilizes multiprocessing to distribute computation during the inference steps. Following standard Julia, more processes can be added as follows: +```julia +# load the required packages into procid()==1 +using Flux, RoME, Caesar, RoMEPlotting + +# then start more processes +using Distributed +addprocs(8) # note this yields 6*8=40 possible processing threads + +# now make sure all code is loaded everywhere (for separate memory cases) +@everywhere using Flux, RoME, Caesar +``` + +It might also be convenient to warm up some of the Just-In-Time compiling: +```julia +# solve a few graphs etc, to get majority of solve code compiled before running a robot. +[warmUpSolverJIT() for i in 1:3]; +``` + +## Start-up Time + +The best way to avoid compile time (when not developing) is to use the established Julia "first time to plot" approach based on PackageCompiler.jl, and more details are provided at [Ahead of Time compiling](@ref compile_binaries). + +## Multithreading + +Julia has strong support for shared-memory multithreading. The most sensible breakdown into threaded work is either within each factor calculation or across individual samples of a factor calculation. Either of these cases require some special considerations. + +### Threading Within the Residual + +A factor residual function itself can be broken down further into threaded operations. For example, see many of the features available at [JuliaSIMD/LoopVectorization.jl](https://github.com/JuliaSIMD/LoopVectorization.jl). It is recommended to keep memory allocations down to zero, since the solver code will call on the factor samping and residual funtions mulitple times in random access. Also keep in mind the interaction between conventional thread pool balancing and the newer [PARTR cache senstive automated thread scheduling](https://julialang.org/blog/2019/07/multithreading/). + +### Threading Across Parallel Samples + +IncrementalInference.jl internally has the capability to span threads across samples in parallel computations during convolution operations. Keep in mind which parts of residual factor computation is shared memory. Likely the best course of action is for the factor definition to pre-allocate `Threads.nthreads()` many memory blocks for factor in-place operations. + +To use this feature, IIF must be told that there are no data race concerns with a factor. The current API uses a keyword argument on [`addFactor!`](@ref): +```julia +addFactor!(fg, [:x0; :x1], MyFactor(...); threadmodel=MultiThreaded) +``` + +!!! warning + The current IIF factor multithreading interface is likely to be reworked/improved in the near future (penciled in for 1H2022). + +See page [Custom Factors](@ref custom_factors) for details on how factor computations are represented in code. Regarding threading, consider for example `OtherFactor.userdata`. The residual calculations from different threads might create a data race on `userdata` for some volatile internal computation. In that case it is recommended the to instead use `Threads.nthreads()` and `Threads.threadid()` to make sure the shared-memory issues are avoided: +```julia +struct MyThreadSafeFactor{T <: SamplableBelief} <: IIF.AbstractManifoldMinimize + Z::T + inplace::Vector{MyInplaceMem} +end + +# helper function +MyThreadSafeFactor(z) = MyThreadSafeFactor(z, [MyInplaceMem(0) for i in 1:Threads.nthreads()]) + +# in residual function just use `thr_inplace = cfo.factor.inplace[Threads.threadid()]` +``` + +!!! note + Beyond the cases discussed above, other features in the IncrementalInference.jl code base (especially regarding the Bayes tree) are already multithreaded. \ No newline at end of file diff --git a/docs/src/concepts/solving_graphs.md b/docs/src/concepts/solving_graphs.md new file mode 100644 index 000000000..267b3caa8 --- /dev/null +++ b/docs/src/concepts/solving_graphs.md @@ -0,0 +1,30 @@ +# [Solving Graphs](@id solving_graphs) + +When you have built the graph, you can call the solver to perform inference with the following: + +```julia +# Perform inference +tree = solveTree!(fg) +``` + +The returned Bayes (Junction) `tree` object is described in more detail on [a dedicated documentation page](https://juliarobotics.org/Caesar.jl/latest/principles/bayestreePrinciples/), while `smt` and `hist` return values most closely relate to development and debug outputs which can be ignored during general use. Should an error occur during, the exception information is easily accessible in the `smt` object (as well as file logs which default to `/tmp/caesar/`). + +```@docs +solveTree! +``` + +## Using Incremental Updates (Clique Recycling I) + +One of the major features of the MM-iSAMv2 algorithm (implemented by [IncrementalInference.jl](http://www.github.com/JuliaRobotics/IncrementalInference.jl)) is reducing computational load by recycling and marginalizing different (usually older) parts of the factor graph. In order to utilize the benefits of recycing, the previous Bayes (Junction) tree should also be provided as input (see fixed-lag examples for more details): +```julia +tree = solveTree!(fg, tree) +``` + +## Using Clique out-marginalization (Clique Recycling II) + +When building sysmtes with limited computation resources, the out-marginalization of cliques on the Bayes tree can be used. This approach limits the amount of variables that are inferred on each solution of the graph. This method is also a compliment to the above Incremental Recycling -- these two methods can work in tandem. There is a default setting for a FIFO out-marginalization strategy (with some additional tricks): +```julia +defaultFixedLagOnTree!(fg, 50, limitfixeddown=true) +``` + +This call will keep the latest 50 variables fluid for inference during Bayes tree inference. The keyword `limitfixeddown=true` in this case will also prevent downward message passing on the Bayes tree from propagating into the out-marginalized branches on the tree. A later page in this documentation will discuss how the inference algorithm and Bayes tree aspects are put together. \ No newline at end of file diff --git a/docs/src/concepts/using_julia.md b/docs/src/concepts/using_julia.md new file mode 100644 index 000000000..cc0f83659 --- /dev/null +++ b/docs/src/concepts/using_julia.md @@ -0,0 +1,130 @@ +# Using Julia + +While Caesar.jl is accessible from various programming languages, this page describes how to use Julia, existing packages, multi-process and multi-threading features, and more. A wealth of general Julia resources are available in the Internet, see [`www.julialang.org](http://www.julialang.org) for more resources. + +If you are familar with Julia, feel free to skip over to the next page. + +## Julia REPL and Help + +Julia's documentation on the REPL can [be found here](https://docs.julialang.org/en/v1/stdlib/REPL/). As a brief example, the REPL in a terminal looks as follows: + +```bash +$ julia -O3 + _ + _ _ _(_)_ | Documentation: https://docs.julialang.org + (_) | (_) (_) | + _ _ _| |_ __ _ | Type "?" for help, "]?" for Pkg help. + | | | | | | |/ _` | | + | | |_| | | | (_| | | Version 1.6.3 (2021-09-23) + _/ |\__'_|_|_|\__'_| | Official https://julialang.org/ release +|__/ | + +julia> ? # upon typing ?, the prompt changes (in place) to: help?> + +help?> string +search: string String Cstring Cwstring RevString randstring bytestring SubString + + string(xs...) + + Create a string from any values using the print function. + ... +``` + +The `-O 3` is for level 3 code compilation optimization and is a useful habit for slightly faster execution, but slightly slower first run just-in-time compilation of any new function. + + +## Loading Packages + +Assuming you just loaded an empty REPL, or at the start of a script, or working inside the VSCode IDE, the first thing to do is load the necessary Julia packages. Caesar.jl is an umbrella package potentially covering over 100 Julia Packages. For this reason the particular parts of the code are broken up amongst more focussed *vertical purpose* library packages. Usually for Robotics either `Caesar` or less expansive `RoME` will do. Other non-Geometric sensor processing applications might build in the MM-iSAMv2, Bayes tree, and DistributedFactorGraph libraries. Any of these packages can be loaded as follows: + +```julia +# umbrella containing most functional packages including RoME +using Caesar +# contains the IncrementalInference and other geometric manifold packages +using RoME +# contains among others DistributedFactorGraphs.jl and ApproxManifoldProducts.jl +using IncrementalInference +``` + +### Optional Package Loading + +Many of these packages have additional features that are not included by default. For example, the [Flux.jl](https://fluxml.ai/Flux.jl/stable/) machine learning package will introduce several additional features when loaded, e.g.: +```julia +julia> using Flux, RoME + +[ Info: IncrementalInference is adding Flux related functionality. +[ Info: RoME is adding Flux related functionality. +``` + +For completeness, so too with packages like `Images.jl`, `RobotOS.jl`, and others: +```julia +using Caesar, Images +``` + +## Running Unit Tests Locally + +Unit tests can further be performed for the upstream packages as follows -- **NOTE** first time runs are slow since each new function call or package must first be precompiled. These test can take up to an hour and may have occasional stochastic failures in any one of the many tests being run. Thus far we have accepted occasional stochasticly driven numerical events---e.g. a test event might result in `1.03 < 1`---rather than making tests so loose such that actual bugs are missed. Strictly speaking, we should repeat tests 10 times over with tighter tolerances, but that would require hundreds or thousands of cloud CI hours a week. +```julia +juila> ] # activate Pkg manager + +# the multimodal incremental smoothing and mapping solver +(v1.6) pkg> test IncrementalInference +... +# robotics related variables and factors to work with IncrementalInference -- can be used standalone SLAM system +(v1.6) pkg> test RoME +... +# umbrella framework with interaction tools and more -- allows stand alone and server based solving +(v1.6) pkg> test Caesar +... +``` + +## Install Repos for Development + +Alternatively, the `dev` command: +```julia +(v1.6) pkg> dev https://github.com/JuliaRobotics/Caesar.jl + +# Or fetching a local fork where you push access +# (v1.6) pkg> dev https://github.com/dehann/Caesar.jl +``` + +!!! warn + Development packages are NOT managed by Pkg.jl, so you have to manage this Git repo manually. Development packages can usually be found at, e.g. `Caesar` + ``` + ~/.julia/dev/Caesar + ``` + +If you'd like to modify or contribute then feel free to fork the specific repo from JuliaRobotics, complete the work on branches in the fork as is normal with a Git workflow and then submit a PR back upstream. We try to keep PRs small, specific to a task and preempt large changes by first merging smaller non-breaking changes and finally do a small switch over PR. We also follow a backport onto `release/vX.Y` branch strategy with common `main || master` branch as the "lobby" for shared development into which individual single responsibility PRs are merged. Each PR, the `main` development lobby, and stable `release/vX.Y` branches are regularly tested through Continuous Integration at each of the repsective packages. + +!!! note + Binary compilation and fast "first-time-to-plot" can be done through [PackageCompiler.jl, see here for more details](@ref compile_binaries). + +## Julia Command Examples + +Run Julia in REPL (console) mode: +```julia +$ julia +julia> println("hello world") +"hello world" +``` + +Maybe a script, or command: + +``` +user@...$ echo "println(\"hello again\")" > myscript.jl +user@...$ julia myscript.jl +hello again +user@...$ rm myscript.jl + +user@...$ julia -e "println(\"one more time.\")" +one more time. +user@...$ julia -e "println(\"...testing...\")" +...testing... +``` + +!!! note + When searching for Julia related help online, use the phrase 'julialang' instead of just 'julia'. For example, search for 'julialang workflow tips' or 'julialang performance tips'. + Also, see [FAQ - Why are first runs slow?](https://www.juliarobotics.org/Caesar.jl/latest/faq/#Just-In-Time-Compiling-(i.e.-why-are-first-runs-slow?)-1), which is due to Just-In-Time/Pre compiling and caching. +## Next Steps + +Although Caesar is Julia-based, it provides multi-language support with a ZMQ interface. This is discussed in [Caesar Multi-Language Support](multilang.md). Caesar.jl also supports various visualizations and plots by using Arena, RoMEPlotting, and Director. This is discussed in [Visualization with Arena.jl and RoMEPlotting.jl](arena_visualizations.md). diff --git a/docs/src/concepts/why_nongaussian.md b/docs/src/concepts/why_nongaussian.md new file mode 100644 index 000000000..edaebfae0 --- /dev/null +++ b/docs/src/concepts/why_nongaussian.md @@ -0,0 +1,21 @@ +# [Why/Where does non-Gaussian data come from?](@id why_nongaussian) + +Gaussian error models in measurement or data cues will only be Gaussian (normally distributed) if all physics/decisions/systematic-errors/calibration/etc. has a correct algebraic model in all circumstances. Caesar.jl and MM-iSAMv2 is heavily focussed on state-estimation from a plethora of heterogenous data that may not yet have perfect algebraic models. Four major categories of non-Gaussian errors have thus far been considered: +- Uncertain decisions (a.k.a. data association), such as a robot trying to decide if a navigation loop-closure can be deduced from a repeat observation of a similar object or measurement from current and past data. These issues are commonly also referred to as multi-hypothesis. +- Underdetermined or underdefined systems where there are more variables than constraining measurements to fully define the system as a single mode---a.k.a solution ambiguity. For example, in 2D consider two range measurements resulting in two possible locations through trilateration. +- Nonlinearity. For example in 2D, consider a Pose2 odometry where the orientation is uncertain: The resulting belief of where a next pose might be (convolution with odometry factor) results in a banana shape curve, even though the entire process is driven by assumed Gaussian belief. +- Physics of the measurement process. Many measurement processes exhibit non-Gaussian behaviour. For example, acoustic/radio time-of-flight measurements, using either pulse-train or matched filtering, result in an "energy intensity" over time/distance of what the range to a scattering-target/source might be--i.e. highly non-Gaussian. + +## Next Steps + +Quick links to related pages: + +```@contents +Pages = [ + "installation_environment.md" + "concepts/concepts.md" + "concepts/building_graphs.md" + "concepts/2d_plotting.md" +] +Depth = 1 +``` diff --git a/docs/src/concepts/zero_install.md b/docs/src/concepts/zero_install.md new file mode 100644 index 000000000..f04e8fa5c --- /dev/null +++ b/docs/src/concepts/zero_install.md @@ -0,0 +1,3 @@ +## Using The NavAbility Cloud + +Coming soon! See [NavAbility.io](https://www.navability.io) for details. These features will include Multi-session/agent support. diff --git a/docs/src/dev/known_issues.md b/docs/src/dev/known_issues.md index 337c89973..2eb4e95b3 100644 --- a/docs/src/dev/known_issues.md +++ b/docs/src/dev/known_issues.md @@ -3,4 +3,23 @@ This page is used to list known issues: - Arena.jl is fairly behind on a number of updates and deprecations. Fixes for this are planned 2021Q2. -- RoMEPlotting.jl main features like `plotSLAM2D` are working, but some of the other features are not fully up to date with recent changes in upstream packages. This too will be updated around Summer 2021. \ No newline at end of file +- RoMEPlotting.jl main features like `plotSLAM2D` are working, but some of the other features are not fully up to date with recent changes in upstream packages. This too will be updated around Summer 2021. + +## Features To Be Restored + +### Install 3D Visualization Utils (e.g. Arena.jl) + +3D Visualizations are provided by [Arena.jl](https://github.com/JuliaRobotics/Arena.jl) as well as development package Amphitheater.jl. +Please follow instructions on the [Visualizations page](concepts/arena_visualizations.md) for a variety of 3D utilities. + +!!! note + Arena.jl and Amphitheater.jl are currently being refactored as part of the broader DistributedFactorGraph migration, the features are are in beta stage (1Q2020). + +Install the latest `master` branch version with +```julia +(v1.5) pkg> add Arena#master +``` + +## Install "Just the ZMQ/ROS Runtime Solver" (Linux) + +Work in progress (see issue [#278](https://github.com/JuliaRobotics/Caesar.jl/issues/278)). diff --git a/docs/src/examples/basic_definingfactors.md b/docs/src/examples/basic_definingfactors.md index 8f5d4a51e..bc3a6d9cc 100644 --- a/docs/src/examples/basic_definingfactors.md +++ b/docs/src/examples/basic_definingfactors.md @@ -1,6 +1,18 @@ -# Defining New Factors +# [Custom Factors](@id custom_factors) -Julia's type inference allows overloading of member functions outside a module. Therefore new factors can be defined at any time. To better illustrate, in this example we will add new factors into the `Main` context **after** construction of the factor graph has already begun. +Julia's type inference allows overloading of member functions outside a module. Therefore new factors can be defined at any time. + + +| Required | Brief description | +|:------------------------------------------|:-------------------------------------------------------------------------------------- | +| `MyFactor` struct | Prior (`<:AbstractPrior`) or Relative (`<:AbstractManifoldMinimize`) factor definition | +| `getManifold` | The manifold of the factor | +| `(cfo::CalcFactor{<:MyFactor})` | Factor residual function | +| **Optional methods** | **Brief description** | +| `getSample(cfo::CalcFactor{<:MyFactor})` | Get a sample from the factor | + + +To better illustrate, in this example we will add new factors into the `Main` context **after** construction of the factor graph has already begun. ```julia using IncrementalInference @@ -41,29 +53,24 @@ end # import IncrementalInference: getSample # sampling function -getSample(cfo::CalcFactor{<:MyPrior}, N::Int=1) = (reshape(rand(cfo.factor.Z,N),1,:), ) +getSample(cfo::CalcFactor{<:MyPrior}) = rand(cfo.factor.Z,1)[:] ``` Note the following critical aspects that allows IIF to use the new definition: - `<:AbstractPrior` as a unary factor that introduces absolute (or gauge) information about only one variable. - `getSample` is overloaded with dispatch on: - - `(cfo::CalcFactor{<:MyPrior}, ::Int)` -- `getSample` must return a `::Tuple`, even if there is only stochastic value (as is the case above). - - The first value in the tuple is special, and must be of type `<:AbstractMatrix{<:Real}`. We ensure that with the `reshape`. - -IIF internally uses the number of rows in the first element of the `getSample` return tuple (i.e. the matrix) to extract the measurement dimension for this factor. In this case it is 1 dimensional. - -!!! note - `getSample` works similar for factors below. If the measurement dimension is 2D (i.e. `zDim=2`) then you should not use `reshape(..., 1,N)` which would force the samples into a 1 row matrix. The `reshape(.., 1,N)` in the above example is added as convenient way to convert from the `rand` return value a `::Array{Float64,1}` to the required `::Array{Float64,2}` type. + - `(cfo::CalcFactor{<:MyPrior})` +- `getSample` must return a point on the manifold for `<:AbstractPrior` factors that matches the point representation of the variable. -To recap, the new `getSample` function in this example factor returns a measurement which is of type `::Tuple{::Matrix{Float64}}`. The `::Tuple` is slightly clunky but was borne out of necessity to allow for versatility when multiple values from sampling are used during residual function evaluation. Previous uses include cases such as `::Tuple{<:Matrix, <:Vector, <:Function}`. +To recap, the new `getSample` function in this example factor returns a measurement which is of type `::Vector{Float64}}`. ### Ready to Use This new prior can now readily be added to an ongoing factor graph: ```julia # lets generate a random nonparametric belief -pts = 8 .+ 2*rand(1,75) + +pts = [samplePoint(getManifold(ContinuousEuclid{1}), Normal(8.0,2.0)) for _=1:75] someBelief = manikde!(pts, ContinuousEuclid{1}) # and build your new factor as an object @@ -72,7 +79,7 @@ myprior = MyPrior(someBelief) and add it to the existing factor graph from earlier, lets say: ```julia -addFactor!(fg, :x1, myprior) +addFactor!(fg, [:x1], myprior) ``` Thats it, this factor is now part of the graph. This should be a solvable graph: @@ -94,66 +101,60 @@ The `cfo` object contains the field `.factor::T` which is the type of the user f Many factors already exists in `IncrementalInference`, `RoME`, and `Caesar`. Please see their `src` directories for more details. -## Relative Factors -### One Dimension Roots Example +## Relative Factors on Manifolds +### One Dimension Example -Previously we looked at adding a prior. This section demonstrates the first of two `<:AbstractRelative` factor types. These are factors that introduce only relative information between variables in the factor graph. +Previously we looked at adding a prior. This section demonstrates relative factors on manifold. These are factors that introduce only relative information between variables in the factor graph. -This example is on `<:IIF.AbstractRelativeRoots`. First, lets create the factor as before +Lets look at the Pose2Pose2 factor available in RoME as an example. First, lets create the factor as before ```julia -struct MyFactor{T <: SamplableBelief} <: IIF.AbstractRelativeRoots +struct Pose2Pose2{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize Z::T end -getSample(cfo::CalcFactor{<:MyFactor}, N::Int=1) = (reshape(rand(cfo.factor.Z,N) ,1,N), ) - -function (cfo::CalcFactor{<:MyFactor})( measurement_z, - x1, - x2 ) - # - res = measurement_z - (x2[1] - x1[1]) - return res -end + +DFG.getManifold(::Pose2Pose2) = Manifolds.SpecialEuclidean(2) ``` +Extending the `getSample` method for our new `Pose2Pose2` factor is optional in this case. +The default behavior for sampling on `<:AbstractManifoldMinimize` factors defined on Group Manifolds is: +- The returned value from `getSample` represents a tangent vector at the identity element. +- The `SamplableBelief` shall be in the field `Z` and that shall be enough to fully define the factor. -The selection of `<:IIF.AbstractRelativeRoots`, akin to earlier `<:AbstractPrior`, instructs IIF to find the roots of the provided residual function. That is the one dimensional residual function, `res[1] = measurement - prediction`, is used during inference to approximate the convolution of conditional beliefs from the approximate beliefs of the connected variables in the factor graph. +If more advanced sampling is required, the `getSample` function can be extended. -Important aspects to note, `<:IIF.AbstractRelativeRoots` requires all elements `length(res)` (the factor measurement dimension) to have a feasible zero crossing solution. A two dimensional system will solve for variables where both `res[1]==0` and `res[2]==0`. +```julia +function getSample(cf::CalcFactor{<:Pose2Pose2}) + M = getManifold(cf.factor) + ϵ = getPointIdentity(Pose2) + X = sampleTangent(M, cf.factor.Z, ϵ) + return X +end +``` -!!! note - As of IncrementalInference v0.21, CalcResidual no longer takes a residual as input parameter and should return residual, see IIF#467. +The return type for `getSample` is unrestricted, and will be passed to the residual function "as-is". !!! note - Measurements and variables passed in to the factor residual function do not have the same type as when constructing the factor graph. It is recommended to leave these incoming types unrestricted. If you must define the types, these either are (or will be) of element type relating to the manifold on which the measurement or variable beliefs reside. Probably a vector or manifolds type. Usage can be very case specific, and hence better to let Julia type-inference automation do the hard work for you. The + Default dispatches in `IncrementalInference` will try use `cf.factor.Z` to `samplePoint` on manifold (for `<:AbstractPrior`) or `sampleTangent` (for `<:AbstractRelative`), which simplifies new factor definitions. If, however, you wish to build more complicated sampling processes, then simply define your own `getSample(cf::CalcFactor{<:MyFactor})` function. -### Two Dimension Minimize Example +The selection of `<:IIF.AbstractManifoldMinimize`, akin to earlier `<:AbstractPrior`, instructs IIF to find the minimum of the provided residual function. +That is the one dimensional residual function, `residual = measurement - prediction`, is used during inference to approximate the convolution of conditional beliefs from the approximate beliefs of the connected variables in the factor graph. -The second type is `<:IIF.AbstractRelativeMinimize` which simply minimizes the residual vector of the user factor. This type is useful for partial constraint situations where the residual function is not gauranteed to have zero crossings in all dimensions and the problem is converted into a minimization problem instead: +The returned value (the factor measurement) from getSample will always be passed as the first argument (`X`) in the factor calculation. +The residual function should return a coordinate. ```julia -struct OtherFactor{T <: SamplableBelief} <: IIF.AbstractRelativeMinimize - Z::T # assuming something 2 dimensional - userdata::String # or whatever is necessary -end - -# just illustrating some arbitraty second value in tuple of different size -getSample(cfo::CalcFactor{<:OtherFactor}, N::Int=1) = (rand(cfo.factor.z,N), rand()) - -function (cfo::CalcFactor{<:OtherFactor})(res::AbstractVector{<:Real}, - z, - second_val, - x1, - x2 ) - # - # @assert length(z) == 2 - # not doing anything with `second_val` but illustrating - # not doing anything with `cfo.factor.userdata` either - - # the broadcast operators with automatically vectorize - res = z .- (x1[1:2] .- x1[1:2]) - return res +function (cf::CalcFactor{<:Pose2Pose2})(X, p, q) + M = getManifold(Pose2) + q̂ = Manifolds.compose(M, p, exp(M, identity_element(M, p), X)) + Xc = vee(M, q, log(M, q, q̂)) + return Xc end ``` +!!! note + Measurements and variables passed in to the factor residual function have related but potentially different types during construction or computation. It is recommended to leave these incoming types unrestricted. If you must define the types, make sure to allow sufficient dispatch freedom (i.e. dispatch to concrete types) and not force operations to "non-concrete" types. Usage can be very case specific, and hence better to let Julia type-inference automation do the hard work of inferring the concrete types. + +[//]: # (#TODO ### Advanced Sampling) + ## Special Considerations ### Partial Factors @@ -168,7 +169,7 @@ end # define a helper constructor MyMagnetoPrior(z) = MyMagnetoPrior(z, (3,)) -getSample(cfo::CalcFactor{<:MyMagnetoPrior}, N::Int=1) = (reshape(rand(cfo.factor.Z,N),1,N),) +getSample(cfo::CalcFactor{<:MyMagnetoPrior}) = samplePoint(cfo.factor.Z) ``` Similarly for `<:IIF.AbstractRelativeMinimize`, and note that the Roots version currently does not support the `.partial` option. @@ -187,24 +188,6 @@ At present `cfo` contains three main fields: !!! note The old `.specialSampler` framework has been replaced with the standardized `::CalcFactor` interface. See http://www.github.com/JuliaRobotics/IIF.jl/issues/467 for details. - -### Multithreading - -Julia and therefore IIF has strong support for shared-memory multithreading. The thing to keep in mind is what parts of residual factor computation is shared memory. The most sensible breakdown into threaded work is for separate samples (i.e. `cfo._sampleIdx`) to be calculated in separate threads. The user residual function itself could likely also be broken down further into more threaded operations. - -The example above introduced `OtherFactor.userdata`. This could cause problems if the residual calculations are actively using `userdata` for some volatile internal computation. In that case it might be useful for the user to instead use `Threads.nthreads()` and `Threads.threadid()` to make sure the shared-memory issues are avoided: -```julia -struct OtherFactor{T <: SamplableBelief} <: IIF.AbstractRelativeMinimize - Z::T # assuming something 2 dimensional - inplace::Vector{MyInplaceMem} -end - -# helper function -OtherFactor(z) = OtherFactor(z, [MyInplaceMem(0) for i in 1:Threads.nthreads()]) - -# in residual function just use `thr_inplace = cfo.factor.inplace[Threads.threadid()]` -``` - ## [OPTIONAL] Standardized Serialization To take advantage of features like `DFG.saveDFG` and `DFG.loadDFG` a user specified type should be able to serialize via JSON standards. The decision was taken to require bespoke factor types to always be converted into a JSON friendly `struct` which must be prefixed as type name with `PackedMyPrior{T}`. Similarly, the user must also overload `Base.convert` as follows: diff --git a/docs/src/examples/canonical_graphs.md b/docs/src/examples/canonical_graphs.md new file mode 100644 index 000000000..2ff9f35a5 --- /dev/null +++ b/docs/src/examples/canonical_graphs.md @@ -0,0 +1,18 @@ +# Canonical Graphs + +try tab-completion in the REPL: +```@docs +IncrementalInference.generateGraph_Kaess +IncrementalInference.generateGraph_TestSymbolic +IncrementalInference.generateGraph_CaesarRing1D +IncrementalInference.generateGraph_LineStep +IncrementalInference.generateGraph_EuclidDistance +RoME.generateGraph_Circle +RoME.generateGraph_ZeroPose +RoME.generateGraph_Hexagonal +RoME.generateGraph_Beehive! +RoME.generateGraph_Honeycomb! +RoME.generateGraph_Helix2D! +RoME.generateGraph_Helix2DSlew! +RoME.generateGraph_Helix2DSpiral! +``` \ No newline at end of file diff --git a/docs/src/examples/custom_variables.md b/docs/src/examples/custom_variables.md index 3a2027308..f5297e337 100644 --- a/docs/src/examples/custom_variables.md +++ b/docs/src/examples/custom_variables.md @@ -1,7 +1,6 @@ +# [Custom Variables](@id custom_variables) -## Creating New Variables - -A handy macro can help define new variables which shows a Pose2 example with 3 degrees of freedom: `X, Y, theta`: +A handy macro can help define new variables which shows a Pose2 example with 3 degrees of freedom: ``X, Y, \theta``. Note that we use [Manifolds.jl as fundamental abstraction](https://juliamanifolds.github.io/Manifolds.jl/latest/examples/manifold.html) for numerical operations. Users can choose how to represent data, for example [`RoME.Pose2`](@ref) is defined as a `Manifolds.SpecialEuclidean(2)`, and the default data representation is (but doesn't have to be) `Manifolds.identity_element(SpecialEuclidean(2))` -- i.e. likely an `Manifolds.ArrayPartition` or `Manifolds.ProductRepr` (older). ```@docs @defVariable ``` @@ -12,6 +11,6 @@ addVariable!(fg, :x0, MyVariable) ``` !!! note - See [RoME.jl#244](http://www.github.com/JuliaRobotics/RoME.jl/issues/244) regarding plans to fundamentally integrate with [Manifolds.jl](http://www.github.com/JuliaManifolds/Manifolds.jl) + Since [RoME.jl#244](http://www.github.com/JuliaRobotics/RoME.jl/issues/244) the Caesar.jl system of packages fundamentally integrated with [Manifolds.jl](http://www.github.com/JuliaManifolds/Manifolds.jl). + -The format for defining manifolds is likely to change in the near future (2021Q1), where manual descriptions like `(:Euclid, :Euclid, :Circular)` will be replaced with a more formal definition like `SE2` or `Manifolds.ProductRepr(Euclid{2}, Rotation)`. diff --git a/docs/src/examples/examples.md b/docs/src/examples/examples.md index 4d65e8151..d16c263ff 100644 --- a/docs/src/examples/examples.md +++ b/docs/src/examples/examples.md @@ -1,4 +1,4 @@ -# Examples +# [Examples](@id examples_section) The following examples demonstrate the conceptual operation of Caesar, highlighting specific features of the framework and its use. diff --git a/docs/src/examples/interm_fixedlag_hexagonal.md b/docs/src/examples/interm_fixedlag_hexagonal.md index 365cf511e..8fd44b846 100644 --- a/docs/src/examples/interm_fixedlag_hexagonal.md +++ b/docs/src/examples/interm_fixedlag_hexagonal.md @@ -1,4 +1,4 @@ -# Hexagonal 2D with Fixed-Lag Solving +# [Hexagonal 2D with Fixed-Lag Solving](@id fixedlag_solving) !!! note diff --git a/docs/src/examples/legacy_deffactors.md b/docs/src/examples/legacy_deffactors.md new file mode 100644 index 000000000..f48d7823b --- /dev/null +++ b/docs/src/examples/legacy_deffactors.md @@ -0,0 +1,59 @@ +## Relative Factors (Legacy) +### One Dimension Roots Example + +Previously we looked at adding a prior. This section demonstrates the first of two `<:AbstractRelative` factor types. These are factors that introduce only relative information between variables in the factor graph. + +This example is on `<:IIF.AbstractRelativeRoots`. First, lets create the factor as before +```julia +struct MyFactor{T <: SamplableBelief} <: IIF.AbstractRelativeRoots + Z::T +end +getSample(cfo::CalcFactor{<:MyFactor}, N::Int=1) = (reshape(rand(cfo.factor.Z,N) ,1,N), ) + +function (cfo::CalcFactor{<:MyFactor})( measurement_z, + x1, + x2 ) + # + res = measurement_z - (x2[1] - x1[1]) + return res +end +``` + + +The selection of `<:IIF.AbstractRelativeRoots`, akin to earlier `<:AbstractPrior`, instructs IIF to find the roots of the provided residual function. That is the one dimensional residual function, `res[1] = measurement - prediction`, is used during inference to approximate the convolution of conditional beliefs from the approximate beliefs of the connected variables in the factor graph. + +Important aspects to note, `<:IIF.AbstractRelativeRoots` requires all elements `length(res)` (the factor measurement dimension) to have a feasible zero crossing solution. A two dimensional system will solve for variables where both `res[1]==0` and `res[2]==0`. + +!!! note + As of IncrementalInference v0.21, CalcResidual no longer takes a residual as input parameter and should return residual, see IIF#467. + +!!! note + Measurements and variables passed in to the factor residual function do not have the same type as when constructing the factor graph. It is recommended to leave these incoming types unrestricted. If you must define the types, these either are (or will be) of element type relating to the manifold on which the measurement or variable beliefs reside. Probably a vector or manifolds type. Usage can be very case specific, and hence better to let Julia type-inference automation do the hard work for you. The + +### Two Dimension Minimize Example + +The second type is `<:IIF.AbstractRelativeMinimize` which simply minimizes the residual vector of the user factor. This type is useful for partial constraint situations where the residual function is not gauranteed to have zero crossings in all dimensions and the problem is converted into a minimization problem instead: +```julia +struct OtherFactor{T <: SamplableBelief} <: IIF.AbstractRelativeMinimize + Z::T # assuming something 2 dimensional + userdata::String # or whatever is necessary +end + +# just illustrating some arbitraty second value in tuple of different size +getSample(cfo::CalcFactor{<:OtherFactor}, N::Int=1) = (rand(cfo.factor.z,N), rand()) + +function (cfo::CalcFactor{<:OtherFactor})(res::AbstractVector{<:Real}, + z, + second_val, + x1, + x2 ) + # + # @assert length(z) == 2 + # not doing anything with `second_val` but illustrating + # not doing anything with `cfo.factor.userdata` either + + # the broadcast operators with automatically vectorize + res = z .- (x1[1:2] .- x1[1:2]) + return res +end +``` diff --git a/docs/src/examples/using_images.md b/docs/src/examples/using_images.md new file mode 100644 index 000000000..51e6af381 --- /dev/null +++ b/docs/src/examples/using_images.md @@ -0,0 +1,16 @@ +# Images and AprilTags + +One common use in SLAM is [AprilTags.jl](https://github.com/JuliaRobotics/AprilTags.jl). Please see that repo for documentation on detecting tags in images. Note that Caesar.jl has a few built in tools for working with [Images.jl](https://github.com/JuliaImages/Images.jl) too. + +```julia +using AprilTags +using Images, Caesar +``` + +Which immediately enables a new factor specifically developed for using AprilTags in a factor graph: +```@docs +Caesar.Pose2AprilTag4Corners +``` + +!!! note + More details to follow. \ No newline at end of file diff --git a/docs/src/examples/using_ros.md b/docs/src/examples/using_ros.md index 12eb321c7..6e3527d51 100644 --- a/docs/src/examples/using_ros.md +++ b/docs/src/examples/using_ros.md @@ -1,31 +1,47 @@ -# Using Caesar.jl with ROS +# [ROS Direct](@id ros_direct) Since 2020, Caesar.jl has native support for ROS via the [RobotOS.jl](https://github.com/jdlangs/RobotOS.jl) package. -!!! warning - Note that ROS neotic has switched to Python3 exclusively, and at the time of writing this page we were usinng Python2.7. See the ROS Wiki here: https://wiki.ros.org/UsingPython3 - -!!! note - See ongoing RobotOS.jl discussion on building a direct C++ interface and skipping PyCall.jl entirely: https://github.com/jdlangs/RobotOS.jl/issues/59 - ## Load the ROS Environment Variables -The first thing to ensure is that the ROS environment is loaded in the bash environment before launching Julia, see ["1.5 Environment setup at ros.org"](https://wiki.ros.org/noetic/Installation/Ubuntu), something similar to: +The first thing to ensure is that the ROS environment variables are loaded before launching Julia, see ["1.5 Environment setup at ros.org"](https://wiki.ros.org/noetic/Installation/Ubuntu), something similar to: ``` source /opt/ros/noetic/setup.bash ``` +### Setup a Catkin Workspace + +Assuming you have bespoke msg types, we suggest using a catkin workspace of choice, for example: +```bash +mkdir -p ~/caesar_ws/src +cd ~/caesar_ws/src +git clone https://github.com/pvazteixeira/caesar_ros +``` + +Now build and configure your workspace + +```bash +cd ~/caesar_ws +catkin_make +source devel/setup.sh +``` + +This last command is important, as you must have the workspace configuration in your environment when you run the julia process, so that you can import the service specifications. + ## RobotOS.jl with Correct Python -RobotOS.jl currently using [PyCall.jl](https://github.com/JuliaPy/PyCall.jl) to interface through the `rospy` system. After launching Julia, make sure that PyCall is using the correct Python binary on your local system. In our local setup, we use (assuming `using Distributed`): +RobotOS.jl currently uses [PyCall.jl](https://github.com/JuliaPy/PyCall.jl) to interface through the `rospy` system. After launching Julia, make sure that PyCall is using the correct Python binary on your local system. ```julia -## Prepare python version +# Assuming multiprocess will be used. +using Distributed +# addprocs(4) + +# Prepare python version using Pkg Distributed.@everywhere using Pkg -# ENV["PYTHON"] = "/usr/bin/python3.6" Distributed.@everywhere begin - ENV["PYTHON"] = "/usr/bin/python" + ENV["PYTHON"] = "/usr/bin/python3" Pkg.build("PyCall") end @@ -132,15 +148,6 @@ while loop!(bagSubscriber) end ``` -## Using AprilTags.jl and Images.jl - -One common use in SLAM is [AprilTags.jl](https://github.com/JuliaRobotics/AprilTags.jl). Please see that repo for documentation on detecting tags in images. Note that Caesar.jl has a few built in tools for working with [Images.jl](https://github.com/JuliaImages/Images.jl) too. - -```julia -using AprilTags -using Images, Caesar -``` - ## Additional Notes !!! note @@ -152,3 +159,5 @@ using Images, Caesar !!! note Additional notes about tricks that came up during development [is kept in this wiki](https://github.com/JuliaRobotics/Caesar.jl/wiki/ROS-PoC). +!!! note + See ongoing RobotOS.jl discussion on building a direct C++ interface and skipping PyCall.jl entirely: https://github.com/jdlangs/RobotOS.jl/issues/59 diff --git a/docs/src/faq.md b/docs/src/faq.md index ef95c09ca..cae2dc408 100644 --- a/docs/src/faq.md +++ b/docs/src/faq.md @@ -34,12 +34,9 @@ See the JuliaCon presentation by [rdeits here](https://www.youtube.com/watch?v=d ## Can Caesar.jl be used in other languages beyond Julia? Yes. The Caesar.jl project is expressly focused on making this algorithmic code available to [C/Fortran](https://docs.julialang.org/en/v1/manual/calling-c-and-fortran-code/)/[C++](https://juliacomputing.com/blog/2017/12/01/cxx-and-cxxwrap-intro.html)/C#/[Python](https://github.com/JuliaPy/PyCall.jl)/[Java](https://github.com/JuliaInterop/JavaCall.jl)/JS. Julia itself offers [many additional interops](https://github.com/JuliaInterop). ZMQ and HTTP/WebSockets are the standardized interfaces of choice, please see [details at the multi-language section](https://www.juliarobotics.org/Caesar.jl/latest/concepts/multilang/)). Consider opening issues or getting in touch for more information. -### Static, Shared Object `.so` Compilation +### Can Julia Compile Binaries / Shared Libraries -Packages are already compiled to static objects (`.ji` files), but can also be compiled to more common `.so` files. See [this AOT vs JIT compiling blog post](https://juliacomputing.com/blog/2016/02/09/static-julia.html) for a deeper discussion. Also see [this Julia Binaries Blog](https://medium.com/@sdanisch/compiling-julia-binaries-ddd6d4e0caf4). See recent dedicated [issue tracker here](https://github.com/JuliaRobotics/RoME.jl/issues/288). Initial work is for system image is [described in the docs here](https://juliarobotics.org/Caesar.jl/latest/installation_environment/#Ahead-Of-Time-Compile-RoME.so-1). - -!!! note - [recent developments announced on discourse.](https://discourse.julialang.org/t/ann-packagecompiler-with-incremental-system-images/20489). Also see new brute force sysimg work at [Fezzik.jl](https://github.com/TsurHerman/Fezzik). +Yes, see the [Compile Binaries Page](@ref compile_binaries). ### Can Julia be Embedded into C/C++ Yes, see [the Julia embedding documentation page](https://docs.julialang.org/en/v1/manual/embedding/index.html). diff --git a/docs/src/index.md b/docs/src/index.md index 25980f7fa..2b69e7785 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -8,70 +8,27 @@ Click here to go to the Caesar.jl Github repo: [![source](https://img.shields.io/badge/source-code-yellow.svg)](https://github.com/JuliaRobotics/Caesar.jl) -## Introduction -Caesar is an open-source robotic software stack aimed at localization and mapping for robotics, using non-Gaussian graphical model state-estimation techniques. Ths factor graph method is well suited to combining heterogeneous and ambiguous sonsor data streams. The focus is predominantly on geometric/spatial/semantic estimation tasks related to simultaneous localization and mapping (SLAM). The software is also highly extensible and well suited to a variety of estimation/filtering-type tasks — especially in non-Gaussian/multimodal settings. +Caesar.jl is a community project to facilate software technology development for localization and mapping from multiple sensor data, and multiple sessions or human / semi-autonomous / autonomous agents. This software is being developed with broadly [Industry 4.0](https://en.wikipedia.org/wiki/Fourth_Industrial_Revolution), Robotics, and [Work of the Future](https://workofthefuture.mit.edu/) in mind. Caesar.jl is an "umbrella package" to combine many other libraries from across the Julia package ecosystem. -Caesar.jl addresses numerous issues that arise in prior SLAM solutions, including: -- solving under-defined systems, -- inference with non-Gaussian measurements, -- standard features for natively handling ambiguous data association and multi-hypotheses, -- simplifying bespoke factor development, -- centralized (or peer-to-peer distributed) factor-graph persistence, -- federated multi-session/agent reduction, and many more. +[NavAbility.io](http://www.navability.io) helps the with administration and support of the Caesar.jl community, please reach out for any additional information (info@navability.io), or using the community links provided below. -Caesar.jl is community orientef and originates from research work at MIT, see the [literature reference page](https://www.juliarobotics.org/Caesar.jl/latest/refs/literature/) for more information. +# Origins and Ongoing Research -[NavAbility](https://www.navability.io/) administers the Caesar/RoME/IncrementalInference packages, please contact [info@navability.io](https://navability.io/contact/) for more details. +Caesar.jl developed as a spin-out project from MIT's Computer Science and Artificial Intelligence Laboratory. See related works on [the literature page](https://www.juliarobotics.org/Caesar.jl/latest/refs/literature/). Many future directions are in the works -- including fundamental research, implementation quality/performance, and system integration. -Ongoing work on this project focuses on the open development of a stable, reliable, verified, user-friendly, and growing library that is well suited to various data-fusion / state-estimation aspects of robotics and autonomy in [non-Gaussian/multi-modal](https://juliarobotics.org/Caesar.jl/latest/concepts/concepts/#Why/Where-does-non-Gaussian-data-come-from?-1) data processing. - -## A Few Highlights - -The Caesar framework offers these and other features: -* Distributed Factor Graph representation deeply-coupled with an on-Manifold probabilistic algebra language; -* Localization using [MM-iSAMv2](http://www.juliarobotics.org/Caesar.jl/latest/refs/literature/#Related-Literature-1) - * Multi-core inference supporting `Pose2, Pose3, Point2, Point3, Multi-modal (multi-hypothesis), IMU preintegration, KDE density, intensity map, partial constraints, null hypothesis, etc`. -* Native multi-modal (hypothesis) representation in the factor-graph, see [Data Association and Hypotheses](@ref): - * Multi-modal and non-parametric representation of constraints; - * Gaussian distributions are but one of the many representations of measurement error; -* Out-of-library extendable for [Creating New Variables and Factors](@ref); -* Natively supports legacy Gaussian parametric and max-mixtures solutions; -* Local in-memory solving on the device as well as database-driven centralized solving (micro-service architecture); -* Natively support *Clique Recycling* (i.e. fixed-lag out-marginalization) for continuous operation as well as off-line batch solving, see more at [Using Incremental Updates (Clique Recycling I)](@ref); -* Natively supports [Dead Reckon Tethering](examples/deadreckontether.md); -* Natively supports Federated multi-session/agent solving; -* Native support for `Entry=>Data` blobs [for storing large format data](https://juliarobotics.org/Caesar.jl/latest/concepts/entry_data/). -* Middleware support, e.g. see the [ROS Integration Page](examples/using_ros.md). - -# The Caesar Framework - -The [Caesar.jl](https://github.com/JuliaRobotics/Caesar.jl) package is an "umbrella" framework around other dedicated algorithmic packages. While most of the packages are implemented in native [Julia](http://www.julialang.org/) ([JuliaPro](http://www.juliacomputing.com)), a few dependencies are wrapped C libraries. Note that C/C++ [can be incorporated with zero overhead](https://docs.julialang.org/en/v1/manual/calling-c-and-fortran-code/), such as was done with [AprilTags.jl](http://www.github.com/JuliaRobotics/AprilTags.jl). - -> [FAQ: Why use Julia?](https://www.juliarobotics.org/Caesar.jl/latest/faq/#Why-Julia-1) - -## AMP / IIF / RoME - -Robot motion estimate ([RoME.jl](http://www.github.com/JuliaRobotics/RoME.jl)) can operate in the conventional SLAM manner, using local memory (dictionaries), or alternatively distribute over a persisted [`DistributedFactorGraph.jl`](http://www.github.com/JuliaRobotics/DistributedFactorGraphs.jl) through common serialization and graph storage/database technologies, [see this article as example](http://people.csail.mit.edu/spillai/projects/cloud-graphs/2017-icra-cloudgraphs.pdf) [1.3]. -A variety of 2D plotting, 3D visualization, serialization, middleware, and analysis tools come standard as provided by the associated packages. RoME.jl combines reference frame transformations and robotics SLAM tool around the back-end solver provides by [IncrementalInference.jl](http://www.github.com/JuliaRobotics/IncrementalInference.jl). - -Details about the accompanying packages: -* [IncrementalInference.jl](http://www.github.com/JuliaRobotics/IncrementalInference.jl) supplies the algebraic logic for factor graph inference with Bayes tree and depends on several packages itself. -* [RoME.jl](http://www.github.com/JuliaRobotics/RoME.jl) introduces nodes and factors that are useful to robotic navigation. -* [ApproxManifoldProducts.jl](http://www.github.com/JuliaRobotics/ApproxManifoldProducts.jl) provides on-manifold belief product operations. - -## Visualization (Arena.jl/RoMEPlotting.jl) -Caesar visualization (plotting of results, graphs, and data) is provided by 2D and 3D packages respectively: -* [RoMEPlotting.jl](http://www.github.com/JuliaRobotics/RoMEPlotting.jl) are a set of scripts that provide MATLAB style plotting of factor graph beliefs, mostly supporting 2D visualization with some support for projections of 3D; -* [Arena.jl](https://github.com/dehann/Arena.jl) package, which is a collection of 3D visualization tools. - -## Multilanguage Interops: Caesar SDKs and APIs -The Caesar framework is not limited to direct Julia use. See the [multi-language page](https://www.juliarobotics.org/Caesar.jl/latest/concepts/multilang/) for details. - -> [FAQ: Interop with other languages (not limited to Julia only)](https://www.juliarobotics.org/Caesar.jl/latest/faq/#Is-Caesar.jl-limited-to-Julia?-No.-1) - -## Origins in Fundamental Research +Consider citing our work: +``` +@online{Caesarjl2021, + author = {{P}ackage {C}ontributors and {E}cosystem}, + title = {Caesar.jl, v0.11.0}, + year = {2021}, + doi= {Solver DOI: 10.5281/zenodo.5146222}, + note = {\url{https://github.com/JuliaRobotics/Caesar.jl}} +} +``` +## Community, Issues, Comments, or Help -See related works on [the literature page](https://www.juliarobotics.org/Caesar.jl/latest/refs/literature/). Many future directions are in the works -- including fundamental research, implementation quality/performance, and system integration. Please see/open issues for specific requests or adding comments to an ongoing discussion -- also consult the Caesar.jl Slack channel to follow/engage with community discussions. +Post [Issues](https://github.com/JuliaRobotics/Caesar.jl/issues), or [Discussions](https://github.com/JuliaRobotics/Caesar.jl/discussions) for community help. Maintainers can easily transfer Issues to the best suited package location if necessary. Also see the history of changes and ongoing work can via the [Milestone pages (click through badges here)](https://github.com/JuliaRobotics/Caesar.jl/blob/master/README.md#bleeding-edge-development-status). You can also get in touch via Slack at [![](https://img.shields.io/badge/Invite-Slack-green.svg?style=popout)](https://join.slack.com/t/caesarjl/shared_invite/zt-ucs06bwg-y2tEbddwX1vR18MASnOLsw). !!! note Please help improve this documentation--if something confuses you, chances @@ -81,35 +38,21 @@ See related works on [the literature page](https://www.juliarobotics.org/Caesar. Your changes will be vetted by developers before becoming permanent, so don't worry about whether you might say something wrong. -## Contributors - -We are grateful for many, many contributions within the Julia package ecosystem -- see the `Project.toml` files of `Caesar, Arena, RoME, RoMEPlotting, KernelDensityEstimate, IncrementalInference, NLsolve, LightGraphs, Gadfly, MeshCat` and others for a far reaching list of contributions. - -Consider citing our work: -``` -@misc{caesarjl, - author = "Contributors and Dependencies", - title = "Caesar.jl, v0.10", - year = 2021, - url = "https://github.com/JuliaRobotics/Caesar.jl" -} -``` - -Please file issues via Github for help to resolve problems for everyone. Issues can be transfered moved upstream to the best suited package location. Changes and ongoing work can be tracked via the [Milestone pages (click through badges here)](https://github.com/JuliaRobotics/Caesar.jl/blob/master/README.md#bleeding-edge-development-status). +# JuliaRobotics Code of Conduct -## JuliaRobotics Code of Conduct The Caesar.jl project is part of the JuliaRobotics organization and adheres to the JuliaRobotics [code-of-conduct](https://github.com/JuliaRobotics/administration/blob/master/code_of_conduct.md). - # Next Steps For installation steps, examples/tutorials, and concepts please refer to the following pages: ```@contents Pages = [ + "concepts/why_nongaussian.md" "installation_environment.md" "concepts/concepts.md" + "concepts/building_graphs.md" + "concepts/2d_plotting.md" "examples/examples.md" - "func_ref.md" ] -Depth = 3 +Depth = 1 ``` diff --git a/docs/src/install_viz.md b/docs/src/install_viz.md new file mode 100644 index 000000000..8c297cd31 --- /dev/null +++ b/docs/src/install_viz.md @@ -0,0 +1,11 @@ +# Install Visualization Tools + + +## 2D Plotting, RoMEPlotting.jl + +RoMEPlotting.jl (2D) and Arena.jl (3D) as optional visualization packages: +```julia +(v1.6) pkg> add RoMEPlotting +``` + + diff --git a/docs/src/installation_environment.md b/docs/src/installation_environment.md index 85609ab6b..09e60ec5c 100644 --- a/docs/src/installation_environment.md +++ b/docs/src/installation_environment.md @@ -1,91 +1,23 @@ -# Welcome +# Install Caesar.jl Caesar.jl is one of the packages within the [JuliaRobotics](http://www.juliarobotics.org) community, and adheres to the [code-of-conduct](https://github.com/JuliaRobotics/administration/blob/master/code_of_conduct.md). -## Local Dependencies +## Possible System Dependencies -The following system packages are used by Caesar.jl: +The following (Linux) system packages are used by Caesar.jl: ``` -# required packages +# Likely dependencies sudo apt-get install hdf5-tools imagemagick # optional packages sudo apt-get install graphviz xdot ``` -## New to Julia and want a full Development Install +For [ROS.org](https://www.ros.org/) users, see at least one usage example at [the ROS Direct page](@ref ros_direct). -### Local Installation of Julia +## Installing Julia Packages -Although [Julia](https://julialang.org/) (or [JuliaPro](https://juliacomputing.com/)) can be installed on a Linux computer using the `apt` package manager, we are striving for a fully local installation environment which is highly reproducible on a variety of platforms. - -The easiest method is---via the terminal---to [download the desired](https://julialang.org/downloads/) version of Julia as a binary, extract, setup a symbolic link, and run: - -```bash -cd ~ -mkdir -p .julia -cd .julia -wget https://julialang-s3.julialang.org/bin/linux/x64/1.6/julia-1.6.1-linux-x86_64.tar.gz -tar -xvf julia-1.6.1-linux-x86_64.tar.gz -rm julia-1.6.1-linux-x86_64.tar.gz -cd /usr/local/bin -sudo ln -s ~/.julia/julia-1.6.1/bin/julia julia -``` -!!! note - Feel free to modify this setup as you see fit. - -This should allow any terminal or process on the computer to run the Julia REPL by type `julia` and testing with: - -#### [Optional] Quick Test that Julia is Working - -Run Julia in REPL (console) mode: -```julia -$ julia -julia> println("hello world") -"hello world" -``` - -Maybe a script, or command: - -``` -user@...$ echo "println(\"hello again\")" > myscript.jl -user@...$ julia myscript.jl -hello again -user@...$ rm myscript.jl - -user@...$ julia -e "println(\"one more time.\")" -one more time. -user@...$ julia -e "println(\"...testing...\")" -...testing... - -``` - -!!! note - When searching for Julia related help online, use the phrase 'julialang' instead of just 'julia'. For example, search for 'julialang workflow tips' or 'julialang performance tips'. - Also, see [FAQ - Why are first runs slow?](https://www.juliarobotics.org/Caesar.jl/latest/faq/#Just-In-Time-Compiling-(i.e.-why-are-first-runs-slow?)-1), which is due to Just-In-Time/Pre compiling and caching. - -## Setup VSCode IDE Environment - -[VSCode IDE](https://www.julia-vscode.org/) allows for interactive development of Julia code using the Julia Extension. After installing and running VSCode, install the Julia Language Support Extension: - -```@raw html -

- -

-``` - -In VSCode, open the command pallette by pressing `Ctrl + Shift + p`. There are a wealth of tips and tricks on how to use VSCode. See [this JuliaCon presentation for as a general introduction into 'piece-by-piece' code execution and much much more](https://www.youtube.com/watch?v=IdhnP00Y1Ks). Working in one of the Julia IDEs like VS Code or Juno should feel something like this (Gif borrowed from [DiffEqFlux.jl](https://github.com/SciML/DiffEqFlux.jl)): -```@raw html -

- -

-``` - -There are a variety of useful packages in VSCode, such as `GitLens`, `LiveShare`, and `Todo Browser` as just a few highlights. These *VSCode Extensions* are independent of the already vast JuliaLang Package Ecosystem (see [JuliaObserver.com](https://juliaobserver.com/)). - -## Julia Packages - -The philosophy around Julia packages are discussed at length in the [Julia core documentation](https://docs.julialang.org/en/stable/manual/packages/), where each Julia package relates to a git repository likely found on [Github.com](http://www.github.com). +The philosophy around Julia packages are discussed at length in the [Julia core documentation](https://docs.julialang.org/en/stable/manual/packages/), where each Julia package relates to a git repository likely found on [Github.com](http://www.github.com). Also see [JuliaHub.com](https://juliahub.com/ui/Packages/Caesar/BNbRm) for dashboard-style representation of the broader Julia package ecosystem. To install a Julia package, simply open a `julia` REPL (equally the Julia REPL in VSCode) and type: ```julia @@ -100,7 +32,7 @@ Unregistered latest packages can also be installed with using only the `Pkg.deve # Caesar is registered on JuliaRegistries/General julia> ] (v1.6) pkg> add Caesar -(v1.6) pkg> add Caesar#some-branch +(v1.6) pkg> add Caesar#janes-awesome-fix-branch (v1.6) pkg> add Caesar@v0.10.0 # or alternatively your own local fork (just using old link as example) @@ -109,74 +41,49 @@ julia> ] See [Pkg.jl](https://github.com/JuliaLang/Pkg.jl) for details and features regarding package management, development, version control, virtual environments and much more. -## Running Unit Tests Locally - -Unit tests can further be performed for the upstream packages as follows -- **NOTE** first time runs are slow since each new function call or package must first be precompiled. -```julia -# the multimodal incremental smoothing and mapping solver -(v1.6) pkg> test IncrementalInference -... -# robotics related variables and factors to work with IncrementalInference -- can be used standalone SLAM system -(v1.6) pkg> test RoME -... -# umbrella framework with interaction tools and more -- allows stand alone and server based solving -(v1.6) pkg> test Caesar -... -``` - -## Install Repos for Development - -Alternatively, the `dev` command: -```julia -(v1.6) pkg> dev https://github.com/JuliaRobotics/Caesar.jl -``` - -!!! warn - Development packages are NOT managed by Pkg.jl, so you have to manage this Git repo manually. Development packages can usually be found at, e.g. `Caesar` - ``` - ~/.julia/dev/Caesar - ``` +## Next Steps -If you'd like to modify or contribute then feel free to fork the specific repo from JuliaRobotics, complete the work on branches in the fork as is normal with a Git workflow and then submit a PR back upstream. We try to keep PRs small, specific to a task and preempt large changes by first merging smaller non-breaking changes and finally do a small switch over PR. We also follow a backport onto `release/vX.X` branch strategy with common `master` as the development lobby that builds successfully 99.999% of the time. +The sections hereafter describe [Building](@ref building_graphs), [Interacting], and [Solving](@ref solving_graphs) factor graphs. We also recommend reviewing the various examples available in the [Examples section](@ref examples_section). -## Ahead Of Time Compile RoME.so +## New to Julia -In RoME, run the `compileRoME/compileRoMESysimage.jl` script - -To use RoME with the newly created sysimage, start julia with: -``` -julia -O3 -J ~/.julia/dev/RoME/compileRoME/RoMESysimage.so -``` +### Installing the Julia Binary +Although [Julia](https://julialang.org/) (or [JuliaPro](https://juliacomputing.com/)) can be installed on a Linux computer using the `apt` package manager, we are striving for a fully local installation environment which is highly reproducible on a variety of platforms. -## 2D Plotting, RoMEPlotting.jl +The easiest method is---via the terminal---to [download the desired](https://julialang.org/downloads/) version of Julia as a binary, extract, setup a symbolic link, and run: -RoMEPlotting.jl (2D) and Arena.jl (3D) as optional visualization packages: -```julia -(v1.6) pkg> add RoMEPlotting +```bash +cd ~ +mkdir -p .julia +cd .julia +wget https://julialang-s3.julialang.org/bin/linux/x64/1.6/julia-1.6.3-linux-x86_64.tar.gz +tar -xvf julia-1.6.3-linux-x86_64.tar.gz +rm julia-1.6.3-linux-x86_64.tar.gz +cd /usr/local/bin +sudo ln -s ~/.julia/julia-1.6.3/bin/julia julia ``` +!!! note + Feel free to modify this setup as you see fit. -## Contributing, Issues, or Comments - -Please feel free to open [issues with Caesar.jl](https://github.com/JuliaRobotics/Caesar.jl/issues) or even Fork and Pull Request as required. -General conversations or comments can be made in the [Caesar Gist](https://gist.github.com/dehann/537f8a2eb9cc24d8bbd35ae92cb4d2d2). - - -## Features To Be Restored In Future +This should allow any terminal or process on the computer to run the Julia REPL by type `julia` and testing with: -### Install 3D Visualization Utils (e.g. Arena.jl) +## VSCode IDE Environment -3D Visualizations are provided by [Arena.jl](https://github.com/JuliaRobotics/Arena.jl) as well as development package Amphitheater.jl. -Please follow instructions on the [Visualizations page](concepts/arena_visualizations.md) for a variety of 3D utilities. +[VSCode IDE](https://www.julia-vscode.org/) allows for interactive development of Julia code using the Julia Extension. After installing and running VSCode, install the Julia Language Support Extension: -!!! note - Arena.jl and Amphitheater.jl are currently being refactored as part of the broader DistributedFactorGraph migration, the features are are in beta stage (1Q2020). +```@raw html +

+ +

+``` -Install the latest `master` branch version with -```julia -(v1.5) pkg> add Arena#master +In VSCode, open the command pallette by pressing `Ctrl + Shift + p`. There are a wealth of tips and tricks on how to use VSCode. See [this JuliaCon presentation for as a general introduction into 'piece-by-piece' code execution and much much more](https://www.youtube.com/watch?v=IdhnP00Y1Ks). Working in one of the Julia IDEs like VS Code or Juno should feel something like this (Gif borrowed from [DiffEqFlux.jl](https://github.com/SciML/DiffEqFlux.jl)): +```@raw html +

+ +

``` -## Install "Just the ZMQ/ROS Runtime Solver" (Linux) +There are a variety of useful packages in VSCode, such as `GitLens`, `LiveShare`, and `Todo Browser` as just a few highlights. These *VSCode Extensions* are independent of the already vast JuliaLang Package Ecosystem (see [JuliaObserver.com](https://juliaobserver.com/)). -Work in progress (see issue [#278](https://github.com/JuliaRobotics/Caesar.jl/issues/278)). diff --git a/docs/src/introduction.md b/docs/src/introduction.md new file mode 100644 index 000000000..b560bb14b --- /dev/null +++ b/docs/src/introduction.md @@ -0,0 +1,30 @@ +# Introduction + +Caesar is an open-source robotic software stack aimed at localization and mapping for robotics, using non-Gaussian graphical model state-estimation techniques. Ths factor graph method is well suited to combining heterogeneous and ambiguous sonsor data streams. The focus is predominantly on geometric/spatial/semantic estimation tasks related to simultaneous localization and mapping (SLAM). The software is also highly extensible and well suited to a variety of estimation /filtering-type tasks — especially in non-Gaussian/multimodal settings. Check out a brief description on [why non-Gaussian / multi-modal](@ref why_nongaussian) data processing needs arise. + +# A Few Highlights + +Caesar.jl addresses numerous issues that arise in prior SLAM solutions, including: +* Distributed Factor Graph representation deeply-coupled with an on-Manifold probabilistic algebra language; +* Localization using different algorithms: + * [MM-iSAMv2](http://www.juliarobotics.org/Caesar.jl/latest/refs/literature/#Related-Literature-1) + * Parametric methods, including regular Guassian or Max-Mixtures. + * Other multi-parametric and non-Gaussian algorithms are presently being implemented. +* Solving under-defined systems, +* Inference with non-Gaussian measurements, +* Standard features for natively handling ambiguous data association and multi-hypotheses, + * Native multi-modal (hypothesis) representation in the factor-graph, see [Data Association and Hypotheses](@ref): + * Multi-modal and non-parametric representation of constraints; + * Gaussian distributions are but one of the many representations of measurement error; +* Simplifying bespoke factor development, +* Centralized (or peer-to-peer decentralized) factor-graph persistence, + * i.e. Federated multi-session/agent reduction. +* Multi-CPU inference. +* Out-of-library extendable for [Creating New Variables and Factors](@ref); +* Natively supports legacy Gaussian parametric and max-mixtures solutions; +* Local in-memory solving on the device as well as database-driven centralized solving (micro-service architecture); +* Natively support *Clique Recycling* (i.e. fixed-lag out-marginalization) for continuous operation as well as off-line batch solving, see more at [Using Incremental Updates (Clique Recycling I)](@ref); +* Natively supports [Dead Reckon Tethering](examples/deadreckontether.md); +* Natively supports Federated multi-session/agent solving; +* Native support for `Entry=>Data` blobs [for storing large format data](https://juliarobotics.org/Caesar.jl/latest/concepts/entry_data/). +* Middleware support, e.g. see the [ROS Integration Page](examples/using_ros.md). diff --git a/docs/src/principles/approxConvDensities.md b/docs/src/principles/approxConvDensities.md index 73965fd55..ccde8ef98 100644 --- a/docs/src/principles/approxConvDensities.md +++ b/docs/src/principles/approxConvDensities.md @@ -102,29 +102,28 @@ addFactor!(fg, [:x0;:x1], odo) # note the list is order sensitive The code block above (not solved yet) describes a algebraic setup exactly equivalent to the convolution equation presented at the top of this page. !!! note - IIF does not require the distribution functions to only be parametric, such as Normal, Rayleigh, mixture models, but also allows intensity based values or kernel density estimates. Parametric types are just used here for ease of illustration. To perform an stochastic approximate convolution with the odometry conditional, one can simply call a low level function used the mmisam solver: ```julia -pts = approxConv(fg, :x0x1f1, :x1) +pts = approxConvBelief(fg, :x0x1f1, :x1) |> getPoints ``` -The `approxConv` function call reads as a operation on `fg` which won't influence any values of parameter list (common Julia exclamation mark convention) and must use the first factor `:x0x1f1` to resolve a convolution on target variable `:x1`. Implicitly, this result is based on the current estimate contained in `:x0`. The value of `pts` is a `:;Array{Float64,2}` where the rows represent the different dimensions (1-D in this case) and the columns are each of the different samples drawn from the intermediate posterior (i.e. convolution result). +The `approxConvBelief` function call reads as a operation on `fg` which won't influence any values of parameter list (common Julia exclamation mark convention) and must use the first factor `:x0x1f1` to resolve a convolution on target variable `:x1`. Implicitly, this result is based on the current estimate contained in `:x0`. The value of `pts` is a `::Array{Float64,2}` where the rows represent the different dimensions (1-D in this case) and the columns are each of the different samples drawn from the intermediate posterior (i.e. convolution result). ```@docs -approxConv +approxConvBelief ``` -IIF currently uses kernel density estimation to convert discrete samples into a smooth function estimate -- more details can be found on the function [approximation principles page here](http://www.juliarobotics.org/Caesar.jl/latest/principles/functionApprox/). The sample set can be converted into an on-manifold functional object as follows: +IIF currently uses kernel density estimation to convert discrete samples into a smooth function estimate. The sample set can be converted into an on-manifold functional object as follows: ```julia # create kde object by referencing back the existing memory location pts hatX1 = manikde!(pts, ContinuousScalar) ``` -The functional object `X1` is now ready for other operations such as function evaluation or product computations discussed on [another principles page](http://www.juliarobotics.org/Caesar.jl/latest/principles/multiplyingDensities/). The `ContinuousScalar` manifold is just the real line in Euclidean space, internally denoted as single element tuple `(:Euclid,)`. +The functional object `X1` is now ready for other operations such as function evaluation or product computations discussed on [another principles page](http://www.juliarobotics.org/Caesar.jl/latest/principles/multiplyingDensities/). The `ContinuousScalar` manifold is just `Manifolds.TranslationGroup(1)`. ## `approxDeconv` @@ -133,12 +132,4 @@ Analogous to a 'forward' convolution calculation, we can similarly approximate t approxDeconv ``` -This feature is not yet as feature rich as the `approxConv` function, and also requires further work to improve the consistency of the calculation -- but none the less exists and is useful in many applications. - -## ZMQ Interface [WORK IN PROGRESS] - -> **NOTE** WIP on expanding ZMQ interface: - -In addition, `ZmqCaesar` offers a `ZMQ` interface to the factor graph solution for multilanguage support. This example is a small subset that shows how to use the `ZMQ` infrastructure, but avoids the larger factor graph related calls. - -... +This feature is not yet as feature rich as the `approxConvBelief` function, and also requires further work to improve the consistency of the calculation -- but none the less exists and is useful in many applications. diff --git a/docs/src/principles/bayestreePrinciples.md b/docs/src/principles/bayestreePrinciples.md index 24171a59a..c444785c9 100644 --- a/docs/src/principles/bayestreePrinciples.md +++ b/docs/src/principles/bayestreePrinciples.md @@ -29,7 +29,7 @@ Trees and factor graphs are separated in the implementation, allowing the user t using IncrementalInference # RoME or Caesar will work too ## construct a distributed factor graph object -fg = generateCanonicalFG_Kaess() +fg = generateGraph_Kaess() # add variables and factors # ... diff --git a/examples/dev/BruteForceTreeOrderings.jl b/examples/dev/BruteForceTreeOrderings.jl index 4b8cb1682..dbabe3a65 100644 --- a/examples/dev/BruteForceTreeOrderings.jl +++ b/examples/dev/BruteForceTreeOrderings.jl @@ -21,7 +21,7 @@ using SuiteSparse.CHOLMOD: SuiteSparse_long include(normpath(Base.find_package("IncrementalInference"), "..", "ccolamd.jl")) -fgs = generateCanonicalFG_Kaess() +fgs = generateGraph_Kaess() drawGraph(fgs) diff --git a/examples/dev/scalar/ex5/demtest.jl b/examples/dev/scalar/ex5/demtest.jl index 9af651251..1e3a5a7cf 100644 --- a/examples/dev/scalar/ex5/demtest.jl +++ b/examples/dev/scalar/ex5/demtest.jl @@ -87,10 +87,10 @@ function cb(fg_, lastpose) z_e = elevation(lastpose) # generate noisy measurement - @info "Callback for DEM heatmap priors" lastpose ls(fg_, lastpose) z_e + @info "Callback for DEM LevelSet priors" lastpose ls(fg_, lastpose) z_e # create prior - hmd = HeatmapDensityRegular(img, (x,y), z_e, sigma_e, N=10000, sigma_scale=1) + hmd = LevelSetGridNormal(img, (x,y), z_e, sigma_e, N=10000, sigma_scale=1) pr = PartialPriorPassThrough(hmd, (1,2)) addFactor!(fg_, [lastpose], pr, tags=[:DEM;], graphinit=false, nullhypo=0.1) nothing @@ -204,16 +204,17 @@ union!(pl_.layers, pl_m.layers); pl_ ## redraw z_e level to see if index orders are right locs = (x->getPPE(fg[x], :simulated).suggested[1:2]).(sortDFG(ls(fg))) -# PartialPriorPassThrough type, with a HeatmapDensityRegular +# PartialPriorPassThrough type, with a LevelSetGridNormal z_es = (x->getFactorType(fg[x]).Z.level).(sortDFG(lsf(fg, tags=[:DEM;]))) @cast locs_[j,i] := locs[j][i] Gadfly.plot(x=locs_[:,1], y=locs_[:,2], color=z_es) -## check getLevelSetSigma - -kp, weights, _roi = IIF.getLevelSetSigma(img, z_es[50], sigma_e, x, y) +## check level set +_roi = img .- z_es[50] +# DEPRECATED USE NEW LevelSetGridNormal and HeatmapDensityGrid +# kp, weights = IIF.sampleLevelSetGaussian!(_roi, sigma_e, x, y) Gadfly.plot(x=kp[1,:],y=kp[2,:], color=weights, Geom.Geom.point) diff --git a/examples/dev/scalar/sampler-dev.jl b/examples/dev/scalar/sampler-dev.jl index 90450d58c..86be75cef 100644 --- a/examples/dev/scalar/sampler-dev.jl +++ b/examples/dev/scalar/sampler-dev.jl @@ -34,7 +34,7 @@ imshow(dem) # z_elevation = 0.5, # actual measurement # sigma_elevation = 0.001 # measurement uncertainty -hmd = HeatmapDensityRegular(dem, (x,y), 0.5, 0.001) +hmd = LevelSetGridNormal(dem, (x,y), 0.5, 0.001) # pr = Mixture(PriorPoint2, [hmd; MvNormal([0;0], [....])], [0.5;0.5]) # addFactor!(fg, [:x17;], pr) diff --git a/examples/marine/asv/philos/ImageOnlyStabilization.jl b/examples/marine/asv/philos/ImageOnlyStabilization.jl new file mode 100644 index 000000000..a322d0b48 --- /dev/null +++ b/examples/marine/asv/philos/ImageOnlyStabilization.jl @@ -0,0 +1,207 @@ + +function transformIndexes(bP__::AbstractVector{C}, aTb) where {C <: CartesianIndex{2}} + bV = zeros(3) + bV[3] = 1.0 + + aV = zeros(3) + aP__ = Vector{Vector{Float64}}(undef, length(bP__)) + + for (i,kp) in enumerate(bP__) + bV[1] = kp[1] + bV[2] = kp[2] + + aV[:] = aTb*bV + aP__[i] = aV[1:2] + end + + return aP__ +end + + +function cost(matches, aTb_) + pts1 = (x->x[1]).(matches) .|> x-> [x[1];x[2]] + _pts2 = (x->x[2]).(matches) + pts2 = transformIndexes(_pts2, aTb_) + + total = 0.0 + for (i,p2) in enumerate(pts2) + total += norm( pts1[i] .- p2 ) + end + + return total +end + +function cost_xyr!(xyr, matches, aTb = [1. 0 0; 0 1 0; 0 0 1]) + aTb[1:2,1:2] .= _Rot.RotMatrix(xyr[3]) + aTb[1:2, 3] .= xyr[1:2] + cost(matches, aTb) +end + +# draw matches + +function mosaicMatches(img1, img2, matches) + grid = hcat(img1, img2) + offset = CartesianIndex(0, size(img1, 2)) + map(m -> draw!(grid, LineSegment(m[1], m[2] + offset)), matches) + grid +end + +function getFeaturesDescriptors(img::AbstractMatrix) + # img = parent(img_) + brisk_params = BRISK() + features = Features(Keypoints(imcorner(img, method=harris))) + desc, ret_features = create_descriptor(Gray.(img), features, brisk_params) +end + +function getCornersDescriptors( img::AbstractMatrix; + kparg1 = 12, kparg2=0.3, + size = 256, window = 20, seed = 123 ) + # + brief_params = BRIEF(;size, window, seed) + keypoints = Keypoints(fastcorners(img, kparg1, kparg2)) + desc, ret_features = create_descriptor(img, keypoints, brief_params); +end + +function getKeypointsMatchBRIEF( img1, img2; + kparg1 = 12, kparg2=0.3, threshold=0.1, + size = 256, window = 20, seed = 123 ) + # + desc_1, ret_kps_1 = getCornersDescriptors( img1; kparg1, kparg2, size, window, seed ) + desc_2, ret_kps_2 = getCornersDescriptors( img2; kparg1, kparg2, size, window, seed ) + + match_keypoints(ret_kps_1, ret_kps_2, desc_1, desc_2, threshold) +end + +function getKeypointsMatchBRISK(img1, img2; + threshold=0.1, + kwargs... ) + # + desc_1, ret_kps_1 = getFeaturesDescriptors( img1; kwargs... ) + desc_2, ret_kps_2 = getFeaturesDescriptors( img2; kwargs... ) + + match_keypoints(Keypoints(ret_kps_1), Keypoints(ret_kps_2), desc_1, desc_2, threshold) +end + + +function calcMatchMinimize( img1::AbstractMatrix{<:Gray}, img2::AbstractMatrix{<:Gray}; + keypoint_fnc::Function=getKeypointsMatchBRIEF, + kwargs...) + # + + matches = keypoint_fnc( img1, img2; kwargs... ) + + res = Optim.optimize((xyr)->cost_xyr!(xyr, matches), [0.0;0.0;0.0]) + res.minimizer, matches +end + + +function pushMatchWindowMedian!(iCB::AbstractVector{<:AbstractMatrix{<:Gray}}, + trBuffer::AbstractVector{<:AbstractVector}, + newImg::AbstractMatrix{<:Gray}; + kwargs... ) + # + for bimg in iCB + trans, matches = calcMatchMinimize(newImg, bimg; kwargs...); + push!(trBuffer, trans) + end + + Ti = median((x->x[1]).(trBuffer)) + Tj = median((x->x[2]).(trBuffer)) + Tr = median((x->x[3]).(trBuffer)) + + return [Ti; Tj; Tr] +end + +Base.@kwdef struct BasicImageOnlyStabilization{N} + imgBuf::CircularBuffer{<:AbstractMatrix{<:Gray}} = CircularBuffer{Matrix{Gray{N0f8}}}(N) + trBuf::CircularBuffer{<:AbstractVector{Float64}} = CircularBuffer{Vector{Float64}}(N) +end + +function (bi::BasicImageOnlyStabilization{N})( newImg::AbstractMatrix; + offset::AbstractVector=zeros(3), + roi_i = :, + roi_j = :, + scale_i=1, + scale_j=1, + scale_r=1, + kwargs... ) where N + # + # image tracking must be in grayscale + img_g = Gray.(newImg[roi_i,roi_j]) + # fill buffer with first image + if length(bi.imgBuf) == 0 + for _ in 1:N + push!(bi.imgBuf, img_g) + end + end + tr = pushMatchWindowMedian!(bi.imgBuf, bi.trBuf, img_g ; kwargs... ) + # replace oldest image in buffer with new + push!(bi.imgBuf, img_g) + + rot = ImageTransformations.recenter(_Rot.RotMatrix(scale_r*tr[3] + offset[3]), [Base.size(img_g)...] .÷ 2) # a rotation around the center + + tform = rot ∘ Translation(scale_i*tr[1],scale_j*tr[2]) + warp(newImg, tform, axes(newImg)) +end + + + +function _stabImageSeq( fg, + lbls=sortDFG( ls(fg, tags=[:POSE;]) ); + threshold=0.4, size=1024, window=50, kparg1 = 11, + filecount::Int=0, + folderpath="/tmp/caesar/philos", + filepath=folderpath*"/quickstab_$filecount.mp4", + logentry=false, + scale_j=0, + scale_r=0.8, + roi_i=1:900, + offset=[0;0;0.017*pi], + N::Integer=3 ) + # + + BIOS! = BasicImageOnlyStabilization{N}() + + des = listDataEntrySequence(fg, lbls[1], r"IMG_CENTER", sortDFG) + img = fetchDataImage(fg, lbls[1], des[1]) + + R_ = _Rot.RotMatrix(0.017*pi) + img_ = warp(img, ImageTransformations.recenter(R_, center(img))); + + encoder_options = (crf=23, preset="medium") + framerate=90 + open_video_out(filepath, img, framerate=framerate, encoder_options=encoder_options) do writer + + for lb in lbls + println("stab ", lb) + + des = listDataEntrySequence(fg, lb, r"IMG_CENTER", sortDFG) + imgs = fetchDataImage.(fg, lb, des) + + for (id,img) in enumerate(imgs) + frame = BIOS!(img; roi_i, scale_j, scale_r, offset, + threshold, size, window, kparg1) + # + write(writer, frame) + addData!( fg, :camera, lb, Symbol(:STAB_, des[id]), + Caesar.toFormat(format"PNG", frame), + mimeType="image/png", + description="ImageMagick.readblob(imgBytes)" ) + end + + end + + end + + # store log + if logentry + open(folderpath*"/log.txt", "a") do fid + println(fid, filepath, " : threshold=$threshold, size=$size, window=$window, kparg1=$kparg1, N=$N") + end + end + +end + + + +# \ No newline at end of file diff --git a/examples/marine/asv/philos/IngestROSbag.jl b/examples/marine/asv/philos/IngestROSbag.jl new file mode 100644 index 000000000..f61fdb6bc --- /dev/null +++ b/examples/marine/asv/philos/IngestROSbag.jl @@ -0,0 +1,375 @@ +""" + Proof of concept for Caesar-ROS integration + (check Caesar Docs for details) + https://juliarobotics.org/Caesar.jl/latest/examples/using_ros/ + + Prerequisites: + - source /opt/ros/noetic/setup.bash + - cd ~/thecatkin_ws + - source devel/setup.bash in all 3 terminals + - run roscore in one terminal + - Then run this Julia in another terminal/process. + + Input: + - Make sure the rosbag is in ~/data/Marine/philos_car_far.bag + + Output: + - Generates output dfg tar and data folder at /tmp/caesar/philos + containing data from the bagfile, see below for details. + + Future: + - ROS msg replies + - periodic export of factor graph object +""" + +## Prepare python version +using Distributed +# addprocs(4) + +using Pkg +Distributed.@everywhere using Pkg + +Distributed.@everywhere begin + ENV["PYTHON"] = "/usr/bin/python3" + Pkg.build("PyCall") +end + +using PyCall +Distributed.@everywhere using PyCall + +## INIT +using RobotOS + +# Also rosnode info +# standard types +@rosimport sensor_msgs.msg: PointCloud2 +@rosimport sensor_msgs.msg: NavSatFix +@rosimport sensor_msgs.msg: CompressedImage +# @rosimport nmea_msgs.msg: Sentence +# seagrant type + +# Application specific ROS message types from catkin workspace +# @rosimport seagrant_msgs.msg: radar + +rostypegen() + +## Load Caesar with additional tools + +using Colors +using Caesar +# using Caesar._ROS + +## + +# using RoME +# using DistributedFactorGraphs + +using DistributedFactorGraphs.DocStringExtensions +using Dates +using JSON2 +using BSON +using Serialization +using FixedPointNumbers +using StaticArrays +using ImageMagick, FileIO +using Images + +using ImageDraw + +## + +# /gps/fix 10255 msgs : sensor_msgs/NavSatFix +# /gps/nmea_sentence 51275 msgs : nmea_msgs/Sentence +# /radar_pointcloud_0 9104 msgs : sensor_msgs/PointCloud2 +# /velodyne_points 20518 msgs : sensor_msgs/PointCloud2 + +# function handleGPS(msg, fg) +# end + + +""" + $TYPEDEF +Quick placeholder for the system state - we're going to use timestamps to align all the data. +""" +Base.@kwdef mutable struct SystemState + curtimestamp::Float64 = -1000 + cur_variable::Union{Nothing, DFGVariable} = nothing + var_index::Int = 0 + lidar_scan_index::Int = 0 + max_lidar::Int = 3 + radar_scan_queue::Channel{sensor_msgs.msg.PointCloud2} = Channel{sensor_msgs.msg.PointCloud2}(64) + # SystemState() = new(-1000, nothing, 0, 0, 3) +end + +## + +""" + $(SIGNATURES) +Update the system state variable if the timestamp has changed (increment variable) +""" +function updateVariableIfNeeded(fg::AbstractDFG, systemstate::SystemState, newtimestamp::Float64) + # Make a new variable if so. + if systemstate.curtimestamp == -1000 || systemstate.cur_variable === nothing || systemstate.curtimestamp < newtimestamp + systemstate.curtimestamp = newtimestamp + systemstate.cur_variable = addVariable!(fg, Symbol("x$(systemstate.var_index)"), Pose2, timestamp = unix2datetime(newtimestamp)) + systemstate.var_index += 1 + systemstate.lidar_scan_index = 0 + end + return nothing +end + + + +""" + $SIGNATURES + +Message callback for Radar pings. Adds a variable to the factor graph and appends the scan as a bigdata element. +""" +function handleRadarPointcloud!(msg::sensor_msgs.msg.PointCloud2, fg::AbstractDFG, systemstate::SystemState) + @info "handleRadarPointcloud!" maxlog=10 + + # assume there is still space (previously cleared) + # add new piece of radar point cloud to queue for later processing. + put!(systemstate.radar_scan_queue, msg) + + # check if the queue still has space + if length(systemstate.radar_scan_queue.data) < systemstate.radar_scan_queue.sz_max + # nothing more to do + return nothing + end + + # Full sweep, lets empty the queue and add a variable + # type instability + queueScans = Vector{Any}(undef, systemstate.radar_scan_queue.sz_max) + + # get the first + md = take!(systemstate.radar_scan_queue) + pc2 = Caesar._PCL.PCLPointCloud2(md) + pc_cat = Caesar._PCL.PointCloud(pc2) + + queueScans[1] = pc2 + + for i in 1:length(systemstate.radar_scan_queue.data) + # something minimal, will do util for transforming PointCloud2 next + md = take!(systemstate.radar_scan_queue) + # @info typeof(md) fieldnames(typeof(md)) + pc2 = Caesar._PCL.PCLPointCloud2(md) + pc_ = Caesar._PCL.PointCloud(pc2) + pc_cat = cat(pc_cat, pc_; reuse=true) + + queueScans[i] = (pc2) + end + + # add a new variable to the graph + timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/1.0e9 + systemstate.curtimestamp = timestamp + systemstate.cur_variable = addVariable!(fg, Symbol("x$(systemstate.var_index)"), Pose2, timestamp = unix2datetime(timestamp), tags=[:POSE]) + systemstate.var_index += 1 + + io = IOBuffer() + serialize(io, queueScans) + + # @show datablob = pc # queueScans + # and add a data blob of all the scans + # Make a data entry in the graph + addData!( fg, :radar, systemstate.cur_variable.label, :RADAR_PC2s, + take!(io), # get base64 binary + # Vector{UInt8}(JSON2.write(datablob)), + mimeType="application/octet-stream/julia.serialize", + description="queueScans = Serialization.deserialize(PipeBuffer(readBytes))") + # + + io = IOBuffer() + serialize(io, pc_cat) + + addData!( fg, :radar, systemstate.cur_variable.label, :RADAR_SWEEP, + take!(io), + mimeType="application/octet-stream/julia.serialize", + description="queueScans = Serialization.deserialize(PipeBuffer(readBytes))" ) + # + + # also make and add an image of the radar sweep + img = makeImage!(pc_cat) + addData!( fg, :radar, systemstate.cur_variable.label, :RADAR_IMG, + Caesar.toFormat(format"PNG", img), + mimeType="image/png", + description="ImageMagick.readblob(imgBytes)" ) + # + + nothing +end + +""" + $SIGNATURES + +Message callback for LIDAR point clouds. Adds a variable to the factor graph and appends the scan as a bigdata element. +Note that we're just appending all the LIDAR scans to the variables because we are keying by RADAR. +""" +function handleLidar!(msg::sensor_msgs.msg.PointCloud2, fg::AbstractDFG, systemstate::SystemState) + @info "handleLidar" maxlog=10 + # Compare systemstate and add the LIDAR scans if we want to. + if systemstate.cur_variable === nothing + return nothing + end + timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/1.0e9 + @info "[$timestamp] LIDAR pointcloud sample on $(systemstate.cur_variable.label) (sample $(systemstate.lidar_scan_index+1))" + + # Check if we have enough LIDAR's for this variable + if systemstate.lidar_scan_index >= systemstate.max_lidar + @warn "Ditching LIDAR sample for this variable, already have enough..." + return nothing + end + + # Make a data entry in the graph + ade,adb = addData!(fg, :lidar, systemstate.cur_variable.label, Symbol("LIDAR$(systemstate.lidar_scan_index)"), Vector{UInt8}(JSON2.write(msg)), mimeType="/velodyne_points;dataformat=Float32*[[X,Y,Z]]*32") + + # NOTE: If JSON, then do this to get to Vector{UInt8} - # byteData = Vector{UInt8}(JSON2.write(xyzLidarF32)) + + # Increment LIDAR scan count for this timestamp + systemstate.lidar_scan_index += 1 +end + +""" + $SIGNATURES + +Message callback for Radar pings. Adds a variable to the factor graph and appends the scan as a bigdata element. +""" +function handleGPS!(msg::sensor_msgs.msg.NavSatFix, fg::AbstractDFG, systemstate::SystemState) + @info "handleGPS" maxlog=10 + if systemstate.cur_variable === nothing + # Keyed by the radar, skip if we don't have a variable yet. + return nothing + end + timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/10^9 + # Update the variable if needed + # updateVariableIfNeeded(fg, systemstate, timestamp) + @info "[$timestamp] GPS sample on $(systemstate.cur_variable.label)" + + if :GPS in listDataEntries(fg, systemstate.cur_variable.label) + @warn "GPS sample on $(systemstate.cur_variable.label) already exist, dropping" + return nothing + end + + io = IOBuffer() + JSON2.write(io, msg) + ade,adb = addData!(fg, :gps_fix, systemstate.cur_variable.label, :GPS, take!(io), mimeType="application/json", description="JSON2.read(IOBuffer(datablob))") + +end + + +""" + $SIGNATURES + +Message callback for camera images. +""" +function handleCamera_Center!(msg::sensor_msgs.msg.CompressedImage, fg::AbstractDFG, systemstate::SystemState) + @info "handleCamera_Center!" maxlog=10 + + lbls = ls(fg, tags=[:POSE;]) + if length(lbls) == 0 + return nothing + end + + lb = sortDFG(lbls)[end] + + addData!( fg, :camera, systemstate.cur_variable.label, Symbol(:IMG_CENTER_, msg.header.seq), + msg.data, + mimeType="image/jpeg", + description="ImageMagick.readblob(imgBytes); # "*msg.format*"; "*string(msg.header.stamp) ) + + nothing +end + + +## + +function main(;iters::Integer=50) + dfg_datafolder = "/tmp/caesar/philos" + if isdir(dfg_datafolder) + println("Deleting old contents at: ",dfg_datafolder) + rm(dfg_datafolder; force=true, recursive=true) + end + mkdir(dfg_datafolder) + + @info "Hit CTRL+C to exit and save the graph..." + + init_node("asv_feed") + # find the bagfile + bagfile = joinpath(ENV["HOME"],"data","Marine","philos_car_far.bag") + bagSubscriber = RosbagSubscriber(bagfile) + + # Initialization + fg = initfg() + getSolverParams(fg).inflateCycles=1 + + ds = FolderStore{Vector{UInt8}}(:radar, "$dfg_datafolder/data/radar") + addBlobStore!(fg, ds) + + ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfg_datafolder/data/gps") + addBlobStore!(fg, ds) + + # add if you want lidar also + ds = FolderStore{Vector{UInt8}}(:lidar, "$dfg_datafolder/data/lidar") + addBlobStore!(fg, ds) + + # add if you want lidar also + ds = FolderStore{Vector{UInt8}}(:camera, "$dfg_datafolder/data/camera") + addBlobStore!(fg, ds) + + + # System state + systemstate = SystemState() + + # Enable and disable as needed. + camcen_sub = bagSubscriber("/center_camera/image_color/compressed", sensor_msgs.msg.CompressedImage, handleCamera_Center!, (fg, systemstate) ) + radarpc_sub = bagSubscriber("/broadband_radar/channel_0/pointcloud", sensor_msgs.msg.PointCloud2, handleRadarPointcloud!, (fg, systemstate) ) + # Skipping LIDAR + # lidar_sub = Subscriber{sensor_msgs.msg.PointCloud2}("/velodyne_points", sensor_msgs.msg.PointCloud2, handleLidar!, (fg,systemstate), queue_size = 10) + gps_sub = bagSubscriber("/gnss", sensor_msgs.msg.NavSatFix, handleGPS!, (fg, systemstate)) + + + @info "subscribers have been set up; entering main loop" + # loop_rate = Rate(20.0) + while loop!(bagSubscriber) + iters -= 1 + iters < 0 ? break : nothing + end + + @info "Exiting" + # After the graph is built, for now we'll save it to drive to share. + # Save the DFG graph with the following: + @info "Saving DFG to $dfg_datafolder/dfg" + saveDFG(fg, "$dfg_datafolder/dfg") + +end + +## + + +# Actually run the program and build +main(iters=100000) + + + +## =========================================================================================== +## after the graph is saved it can be loaded and the datastores retrieved + +dfg_datafolder = "/tmp/caesar/philos" + +fg = loadDFG("$dfg_datafolder/dfg") + +ds = FolderStore{Vector{UInt8}}(:radar, "$dfg_datafolder/data/radar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfg_datafolder/data/gps") +addBlobStore!(fg, ds) + +# add if you want lidar also +ds = FolderStore{Vector{UInt8}}(:lidar, "$dfg_datafolder/data/lidar") +addBlobStore!(fg, ds) + +# add if you want lidar also +ds = FolderStore{Vector{UInt8}}(:camera, "$dfg_datafolder/data/camera") +addBlobStore!(fg, ds) + +## \ No newline at end of file diff --git a/examples/marine/asv/philos/JointVisualization.jl b/examples/marine/asv/philos/JointVisualization.jl new file mode 100644 index 000000000..8e11ada22 --- /dev/null +++ b/examples/marine/asv/philos/JointVisualization.jl @@ -0,0 +1,218 @@ + +#= + +Draw radar map in resulting world frame (after solve) + + +=# + +## + + +using Distributed +# addprocs(10) + +using Colors +using Caesar +@everywhere using Caesar + +using DistributedFactorGraphs: listDataEntrySequence + +import Rotations as _Rot +using CoordinateTransformations + +# using DistributedFactorGraphs.DocStringExtensions +using Dates +using JSON2 +# using BSON +using Serialization +using FixedPointNumbers +using Manifolds +using StaticArrays +using TensorCast +using DataStructures + +using ImageMagick, FileIO +using Images +using ImageDraw +using ImageView +using ImageTransformations +using ImageFeatures + +using VideoIO + +using RoMEPlotting +Gadfly.set_default_plot_size(35cm,20cm) + + +using Optim + +## load the solved graph and reattach the datastores + +dfg_datafolder = "/tmp/caesar/philos" + +fg = loadDFG("$dfg_datafolder/results_5/x0_5_x280") + +## + +ds = FolderStore{Vector{UInt8}}(:radar, "$dfg_datafolder/data/radar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfg_datafolder/data/gps") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:lidar, "$dfg_datafolder/data/lidar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:camera, "$dfg_datafolder/data/camera") +addBlobStore!(fg, ds) + + +## structure + +# walk through all the poses in fg +# pull out all world frame radar images +# pull out all camera images at each (not all poses have world location solution) +# draw side by side radar on left and camera on right (more camera than radar images) +# + + +## + +M = SpecialEuclidean(2) +e0 = identity_element(M) + +## + +# get two radar sweep point clouds and rotate them to the world frame +function _fetchDataPointCloudWorld(fg::AbstractDFG, lb::Symbol) + de,db = getData(fg, lb, :RADAR_SWEEP) + # deserialize is not legacy safe, rather use BSON.@save(io, Caesar._PCL.PCLPointCloud2(pc)) + bPC = deserialize(PipeBuffer(db)) + wPb = exp(M, e0, hat(M, e0, getPPE(fg, lb).suggested)) + apply(M, wPb, bPC) +end + + +Base.clamp(img::AbstractMatrix{<:RGB},b::Real,t::Real) = map(px->RGB{N0f8}(clamp(px.r,b,t),clamp(px.g,b,t),clamp(px.b,b,t)), img) + + +function buildPoseImages( fg::AbstractDFG, + lb::Symbol; + # _X_ = 3000, + # _Y_ = 1200, + pattern::Regex=r"IMG_CENTER_", + T = RGB{N0f8}, + color=Gray{Float64}(0.05), + scale_map=1.2, + ppose=zeros(3) ) + # + global img_wPC, imgL + + M = SpecialEuclidean(2) + e0 = identity_element(M) + + R_ = _Rot.RotMatrix(0.0) # 0.017*pi + + framestack = [] + + # fetch the camera images + ents = listDataEntrySequence(fg, lb, pattern, sortDFG) + imgs_lb = fetchDataImage.(fg, lb, ents) + rows,cols = size(imgs_lb[1]) + + # expand the radar map + if isSolved(fg, lb) + wPC__ = _fetchDataPointCloudWorld(fg, lb); + # add traj plotting here (draw points and lines on radar plot) + + nRw = _Rot.RotMatrix(140*pi/180.0) + nTw = ProductRepr([0.0;0.0], nRw) + nPC = apply(M, nTw, wPC__) + __ppe = getPPE(fg, lb).suggested + _ppe = Manifolds.compose(M, nTw, exp(M, e0, hat(M, e0, __ppe))) + ppe = vee(M, e0, log(M, e0, _ppe)) + img_wPC_ = makeImage!(nPC, (-2500,1000),(-2000,1000); rows=trunc(Int, scale_map*rows), color, pose=ppe, ppose=ppose ); + ppose .= ppe + if lb == :x0 + img_wPC = deepcopy(img_wPC_) + else + img_wPC .+= img_wPC_ + end + imgG = (px->RGB(0,10*px,0)).(img_wPC_); + + imgL = RGB.(img_wPC) + imgG + end + + imgs_R = map(img->warp(img, ImageTransformations.recenter(R_, center(img))), imgs_lb) + + frame_ = Matrix{T}(undef, size(imgs_R[1])...) + + # write mosaic frames to the video + for frame in imgs_R + ax, ay = axes(frame) + I = ax.parent .+ ax.offset + J = ay.parent .+ ay.offset + for (i,i_) in enumerate(I), (j,j_) in enumerate(J) + frame_[i,j] = frame[i_,j_] + end + imgF = mosaic([imgL, frame_]; nrow=1) #, npad=50) + push!(framestack, imgF) + end + + # typeof(framestack[1]) + framestack_ = Vector{Matrix{T}}(undef, length(framestack)); + for (i,fr) in enumerate(framestack) + framestack_[i] = clamp(fr, 0,1); + end + + return framestack_ +end + + +## + + + +lbls = sortDFG(ls(fg)) #[1:41] + +# imgL = nothing # Matrix{RGB{Float64}}() +global img_wPC = nothing +global imgL = nothing + +ppose = zeros(3) +pattern = r"STAB_IMG_CENTER_" + +framestack = buildPoseImages(fg, :x0; ppose, pattern ); + +## + +encoder_options = (crf=23, preset="medium") +framerate=90 +open_video_out("/tmp/caesar/philos/mosaic_stab.mp4", framestack[1], framerate=framerate, encoder_options=encoder_options) do writer + + for (i,lb) in enumerate(lbls) + println("doing all frames for ", lb) + + framestack = buildPoseImages(fg, lb; ppose, pattern ) + + for frame in framestack + # frame_ .= T.(frame) + write(writer, frame) + end + + end # for + +end # video + + +## + +lb = :x48 +des = listDataEntrySequence(fg, lb, r"IMG_CENTER", sortDFG) +imgs = fetchDataImage.(fg, lb, des) + + +## + +# \ No newline at end of file diff --git a/examples/marine/asv/philos/RadarMapping.jl b/examples/marine/asv/philos/RadarMapping.jl new file mode 100644 index 000000000..7b3cfbc7e --- /dev/null +++ b/examples/marine/asv/philos/RadarMapping.jl @@ -0,0 +1,160 @@ + +#= + +Draw radar map in resulting world frame (after solve) + + +=# + +## + + +using Distributed +# addprocs(10) + +using Colors +using Caesar +@everywhere using Caesar + +import Rotations as _Rot + +# using DistributedFactorGraphs.DocStringExtensions +using Dates +using JSON2 +# using BSON +using Serialization +using FixedPointNumbers +using Manifolds +using StaticArrays +using ImageMagick, FileIO +using Images + +using ImageDraw +using ImageView +using ImageTransformations + +using RoMEPlotting +Gadfly.set_default_plot_size(35cm,20cm) + + +## load the solved graph and reattach the datastores + +dfg_datafolder = "/tmp/caesar/philos" + +fg = loadDFG("$dfg_datafolder/results_4/x0_5_x280") + +## + +ds = FolderStore{Vector{UInt8}}(:radar, "$dfg_datafolder/data/radar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfg_datafolder/data/gps") +addBlobStore!(fg, ds) + +# add if you want lidar also +ds = FolderStore{Vector{UInt8}}(:lidar, "$dfg_datafolder/data/lidar") +addBlobStore!(fg, ds) + +# add if you want lidar also +ds = FolderStore{Vector{UInt8}}(:camera, "$dfg_datafolder/data/camera") +addBlobStore!(fg, ds) + + +## visualize the radar data + +if false + imgs = map(x->Gray{N0f8}.(x), fetchDataImage.(fg, sortDFG(ls(fg)), :RADAR_IMG)); + writevideo("/tmp/caesar/philos/radar280.mp4", imgs; fps=5, player="totem") +end + +## + +M = SpecialEuclidean(2) +e0 = identity_element(M) + +## + + +# get two radar sweep point clouds and rotate them to the world frame +function _fetchDataPointCloudWorld(fg::AbstractDFG, lb::Symbol) + de,db = getData(fg, lb, :RADAR_SWEEP) + # deserialize is not legacy safe, rather use BSON.@save(io, Caesar._PCL.PCLPointCloud2(pc)) + bPC = deserialize(PipeBuffer(db)) + wPb = exp(M, e0, hat(M, e0, getPPE(fg, lb).suggested)) + apply(M, wPb, bPC) +end + + +## + +wPC0 = _fetchDataPointCloudWorld(fg, :x0) +img0 = makeImage!(wPC0, (-1500,1500); color=Gray{N0f8}(0.01)) +imshow(img0) + +wPC50 = _fetchDataPointCloudWorld(fg, :x50) +# makeImage!(wPC50, (-1500,1500)) |> imshow + +wPC100 = _fetchDataPointCloudWorld(fg, :x100) +# makeImage!(wPC100, (-2000,2000)) |> imshow + + +## + +_X_ = 3000 +_Y_ = 1250 + +wPC__ = _fetchDataPointCloudWorld.(fg, [Symbol(:x,i) for i in 0:5:280]); +img_wPC_ = map(pc->makeImage!(pc, (-1250,_X_),(-_Y_,_Y_); color=Gray{Float64}(0.05)), wPC__); +img_wPC = +(img_wPC_...); + +imshow(img_wPC) + +## movie sequence in world frame + +IMGS = Vector{Matrix{RGB{Float64}}}() + +img_wPC = RGB.(img_wPC_[1]) +for gim in img_wPC_[2:end] + # green overlay of latest + imgG = (px->RGB(0,10*px,0)).(gim); + imgL = img_wPC + imgG + push!(IMGS, imgL) + + # grow full world map + img_wPC += RGB.(gim) +end + +writevideo("/tmp/caesar/philos/results_4/radarmap.mp4", IMGS; fps=3, player="totem") + + + + +## ================================================================================ +## Keeping a few code fragments below +## ================================================================================ + + +# for l in [Symbol(:x,i) for i in 10:10:280] +# wPC = cat(wPC, _fetchDataPointCloudWorld(fg, l); reuse=true ) +# end +# img_wPC = makeImage!(wPC, (-1250,_X_),(-_Y_,_Y_)) +# imshow(img_wPC) + +## manually north align + +# nPC = apply(M, ProductRepr([0.0;0.0], _Rot.RotMatrix(135*pi/180.0)), wPC) + +# img_nPC = makeImage!(nPC, (-2500,1000),(-2000,1500), color=Gray{N0f8}(0.1)) +# img_nPC[1,1] = Gray(1) +# imshow(img_nPC) + +# ## + +# using ImageMagick, FileIO +# pngimg = toFormat(format"PNG", img_wPC) +# save(dfg_datafolder*"/results_3/wPC.png", img_wPC) + +# ## + + +# \ No newline at end of file diff --git a/examples/marine/asv/philos/RadarOdometry.jl b/examples/marine/asv/philos/RadarOdometry.jl new file mode 100644 index 000000000..87de2d9ac --- /dev/null +++ b/examples/marine/asv/philos/RadarOdometry.jl @@ -0,0 +1,135 @@ + +using Distributed +# addprocs(10) + +using Colors +using Caesar +@everywhere using Caesar + +using DistributedFactorGraphs.DocStringExtensions +using Dates +using JSON2 +using BSON +using Serialization +using FixedPointNumbers +using StaticArrays +using ImageMagick, FileIO +using Images + +using ImageDraw + + +## after the graph is saved it can be loaded and the datastores retrieved + +dfg_datafolder = "/tmp/caesar/philos" + +fg = loadDFG("$dfg_datafolder/dfg") + +ds = FolderStore{Vector{UInt8}}(:radar, "$dfg_datafolder/data/radar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfg_datafolder/data/gps") +addBlobStore!(fg, ds) + +# add if you want lidar also +ds = FolderStore{Vector{UInt8}}(:lidar, "$dfg_datafolder/data/lidar") +addBlobStore!(fg, ds) + +# add if you want lidar also +ds = FolderStore{Vector{UInt8}}(:camera, "$dfg_datafolder/data/camera") +addBlobStore!(fg, ds) + +## visualize the radar data + +if false + imgs = map(x->Gray{N0f8}.(x), fetchDataImage.(fg, sortDFG(ls(fg)), :RADAR_IMG)); + writevideo("/tmp/caesar/philos/radar5.mp4", imgs; fps=5, player="totem") +end + + +## For this example, lets only add a few transform factors between poses, say every 10th pose + +# set case specific parameters +getSolverParams(fg).inflateCycles=1 +getSolverParams(fg).inflation=2.0 + +# switch of all variables +setSolvable!.(fg, ls(fg), 0) + +# select the pose variables to include in the solve +slv = [Symbol("x",i) for i in 0:5:20] # 285 +# setSolvable!.(fg, slv, 1) + +# add a PriorPose2 +if 0==length(lsf(fg, tags=[:ORIGIN])) + addFactor!(fg, [:x0;], PriorPose2(MvNormal([0;0;0.],diagm(0.01*[1;1;1.]))), tags=[:ORIGIN;], solvable=1) +end + +## load the point clouds and create the radar odometry factors + +bw = 2.0 +for (i,lb) in enumerate(slv[1:(end-1)]) + # Cloud 1, removing values near vehicle itself + de,db = getData(fg, lb, :RADAR_SWEEP) + sweep = deserialize(PipeBuffer(db)) # BSON.@load + XY = map(pt->pt.data[1:2], sweep.points) + XY_ = filter(pt->10 < norm(pt), XY) + r1 = manikde!(getManifold(Point2), XY_, bw=[bw;bw]) + + # Cloud2, removing values near vehicle itself + lb_ = slv[i+1] + de,db = getData(fg, lb_, :RADAR_SWEEP) + sweep = deserialize(PipeBuffer(db)) # BSON.@load + XY = map(pt->pt.data[1:2], sweep.points) + XY_ = filter(pt->10 < norm(pt), XY) + r2 = manikde!(getManifold(Point2), XY_, bw=[bw;bw]) + + # create the radar alignment factor + sap = ScatterAlignPose2(;cloud1=r1, cloud2=r2, bw=0.0001, sample_count=200) + + # add the new factor to the graph + addFactor!(fg, [lb; lb_], sap, inflation=0.0, solvable=0, tags=[:RADAR_ODOMETRY], graphinit=false) +end + + +## make a copy of this subgraph for for dev engineering and debugging only + +fg_ = if false + _fg_ = initfg() + getSolverParams(_fg_).inflateCycles=1 + getSolverParams(_fg_).inflation = 2.0 + getSolverParams(_fg_).alwaysFreshMeasurements = false + copyGraph!(_fg_, fg, slv, union((ls.(fg, slv))...)) + _fg_ +else + fg +end + + +## many batch solves of graph copy + +setSolvable!(fg_, :x0, 1) +setSolvable!(fg_, :x0f1, 1) + +tree = buildTreeReset!(fg_) + +let tree = tree + for (i,lb) in enumerate(slv) + # latest pose to solve (factors were set at previous cycle) + setSolvable!(fg_, lb, 1) + if isodd(i) + @info "solve for" lb + tree = solveTree!(fg_, tree; storeOld=true); + saveDFG("/tmp/caesar/philos/results_5/x0_5_$(lb)", fg_) + end + + # set factors for next cycle + if i < length(slv) + btwfc = intersect(ls(fg_, lb), ls(fg_, slv[i+1])) + setSolvable!.(fg_, btwfc, 1) + end + end +end + + +# \ No newline at end of file diff --git a/examples/marine/asv/philos/StabilizeCameraOnly.jl b/examples/marine/asv/philos/StabilizeCameraOnly.jl new file mode 100644 index 000000000..584a90917 --- /dev/null +++ b/examples/marine/asv/philos/StabilizeCameraOnly.jl @@ -0,0 +1,133 @@ + +#= + +Draw radar map in resulting world frame (after solve) + + +=# + +## + + +using Distributed +# addprocs(10) + +using Colors +using Caesar +@everywhere using Caesar + +using DistributedFactorGraphs: listDataEntrySequence + +import Rotations as _Rot +using CoordinateTransformations + +# using DistributedFactorGraphs.DocStringExtensions +using Dates +using JSON2 +# using BSON +using Serialization +using FixedPointNumbers +using Manifolds +using StaticArrays +using TensorCast +using DataStructures + +using ImageMagick, FileIO +using Images +using ImageDraw +using ImageView +using ImageTransformations +using ImageFeatures + +using VideoIO + +using RoMEPlotting +Gadfly.set_default_plot_size(35cm,20cm) + +using ProgressMeter + +using Optim + +## load the solved graph and reattach the datastores + +dfg_datafolder = "/tmp/caesar/philos" + +fg = loadDFG("$dfg_datafolder/results_5/x0_5_x280_stab") + +## + +ds = FolderStore{Vector{UInt8}}(:radar, "$dfg_datafolder/data/radar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfg_datafolder/data/gps") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:lidar, "$dfg_datafolder/data/lidar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:camera, "$dfg_datafolder/data/camera") +addBlobStore!(fg, ds) + + +## + +M = SpecialEuclidean(2) +e0 = identity_element(M) + +## + + +##====================================================================================== + +include(joinpath(@__DIR__,"ImageOnlyStabilization.jl")) + + + +## + +lbls = sortDFG(ls(fg, tags=[:POSE;])) #[1:100] + +_stabImageSeq(fg, lbls; filecount=20, threshold=0.2, kparg1=12, size=512, window=20, logentry=true, N=15, scale_r=0.8) +# _stabImageSeq(fg; filecount=11, threshold=0.2, kparg1=10, logentry=true) + +## + +saveDFG("$dfg_datafolder/results_5/x0_5_x280_stab", fg) + + +## + + +# count = 0 +# let count = count +# @showprogress 1 "sweep" for kp1=[12;], thr=[0.2;0.5;0.8;1.1;] +# count += 1 +# _stabImageSeq(fg, lbls; filecount=count, threshold=thr, kparg1=kp1, logentry=true) +# end +# end + +## + +# T = typeof(IMGS[1]) +# IMGS_ = Vector{T}(undef, length(IMGS)) +# IMGS_ .= IMGS +# writevideo("/tmp/caesar/philos/quickstab.mp4", IMGS_; fps=90) + +## + +# mosaicMatches(imgG[[1;3]]..., matches) |> imshow +# trans, matches = calcMatchMinimize(imgG[[1;2]]...); trans + +## + +# img1 = Gray.(imgs[1]) +# img2 = Gray.(imgs[3]) +# tr, matches = calcMatchMinimize(img1,img2; threshold=0.4, size=1024, window=50) +# mosaicMatches(img1, img2, matches) |> imshow + + + +## + + +# \ No newline at end of file diff --git a/examples/marine/asv/rex/RExFeed.jl b/examples/marine/asv/rex/RExFeed.jl index 6b27d9f5e..ef68421fb 100644 --- a/examples/marine/asv/rex/RExFeed.jl +++ b/examples/marine/asv/rex/RExFeed.jl @@ -20,7 +20,7 @@ using JSON2 # standard types @rosimport sensor_msgs.msg: PointCloud2 @rosimport sensor_msgs.msg: NavSatFix -# @rosimport nmea_msgs.msg: Sentence +@rosimport nmea_msgs.msg: Sentence # seagrant type # OMG, rosmsg list is awesome by the way... # Also rosnode info diff --git a/examples/marine/asv/rex/RExFeed_Bag.jl b/examples/marine/asv/rex/RExFeed_Bag.jl new file mode 100644 index 000000000..3be1e64ff --- /dev/null +++ b/examples/marine/asv/rex/RExFeed_Bag.jl @@ -0,0 +1,318 @@ +""" + Proof of concept for Caesar-ROS integration + (check Caesar wiki for details/instructions) + + To do: + - re-enable JSON replies + s- periodic export of factor graph object + + To run: + - source /opt/ros/noetic/setup.bash + - cd ~/thecatkin_ws + - source devel/setup.bash in all 3 terminals + - run roscore in one terminal + - Make sure the rosbag is in ~/data/Marine/lidar_radar.bag + - and julia RExFeed.jl in another terminal. +""" + +## Prepare python version +using Distributed +# addprocs(4) + +using Pkg +Distributed.@everywhere using Pkg + +Distributed.@everywhere begin + ENV["PYTHON"] = "/usr/bin/python3" + Pkg.build("PyCall") +end + +using PyCall +Distributed.@everywhere using PyCall + +## INIT +using RobotOS + +# standard types +@rosimport sensor_msgs.msg: PointCloud2 +@rosimport sensor_msgs.msg: NavSatFix +# @rosimport nmea_msgs.msg: Sentence +# seagrant type +# OMG, rosmsg list is awesome by the way... +# Also rosnode info +@rosimport seagrant_msgs.msg: radar + +rostypegen() +# No using needed because we're specifying by full name. +# using .sensor_msgs.msg +# using .seagrant_msgs.msg + +## Load Caesar mmisam stuff +using Caesar +using RoME +using DistributedFactorGraphs + +using JSON2 +using DistributedFactorGraphs.DocStringExtensions +using Dates + +## + +# /gps/fix 10255 msgs : sensor_msgs/NavSatFix +# /gps/nmea_sentence 51275 msgs : nmea_msgs/Sentence +# /radar_0 9104 msgs : seagrant_msgs/radar +# /radar_pointcloud_0 9104 msgs : sensor_msgs/PointCloud2 +# /velodyne_points 20518 msgs : sensor_msgs/PointCloud2 + +# function handleGPS(msg, fg) +# end + + +""" + $TYPEDEF +Quick placeholder for the system state - we're going to use timestamps to align all the data. +""" +mutable struct SystemState + curtimestamp::Float64 + cur_variable::Union{Nothing, DFGVariable} + var_index::Int + lidar_scan_index::Int + max_lidar::Int + SystemState() = new(-1000, nothing, 0, 0, 3) +end + +""" + $(SIGNATURES) +Update the system state variable if the timestamp has changed (increment variable) +""" +function updateVariableIfNeeded(fg::AbstractDFG, systemstate::SystemState, newtimestamp::Float64) + # Make a new variable if so. + if systemstate.curtimestamp == -1000 || systemstate.cur_variable === nothing || systemstate.curtimestamp < newtimestamp + systemstate.curtimestamp = newtimestamp + systemstate.cur_variable = addVariable!(fg, Symbol("x$(systemstate.var_index)"), Pose2, timestamp = unix2datetime(newtimestamp)) + systemstate.var_index += 1 + systemstate.lidar_scan_index = 0 + end + return nothing +end + + + +# TODO much room for improvement, and must be consolidated with RobotOS.jl +function _unpackROSMsgType(T::Type, msgdata) + msgT = T() + msgT.header = msgdata[2][:header] + fnms = fieldnames(T) + for nm in fnms + # FIXME, still have to resolve cases where fieldname header is actually used + nm == :header ? continue : nothing + _tostr = JSON2.write(msgdata[2][nm]) + try + setfield!(msgT, nm, JSON2.read(_tostr, typeof(getfield(msgT, nm)))) + catch + @warn "not able to unpack ROS message field" T nm typeof(getfield(msgT, nm)) + @debug _tostr + end + end + msgT +end + +""" + $SIGNATURES + +Message callback for /radar_0. +""" +function handleRadar!(msg::seagrant_msgs.msg.radar, fg::AbstractDFG, systemstate::SystemState) + @info "handleRadar" maxlog=10 + + # if systemstate.cur_variable === nothing + # return nothing + # end + # Update the variable if needed + timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/1.0e9 + updateVariableIfNeeded(fg, systemstate, timestamp) + @info "[$timestamp] RADAR sample on $(systemstate.cur_variable.label)" + + # Make a data entry in the graph - use JSON2 to just write this (really really verbosely) + ade,adb = addData!(fg, :radar, systemstate.cur_variable.label, :RADAR, Vector{UInt8}(JSON2.write(msg))) + +end + +""" + $SIGNATURES + +Message callback for Radar pings. Adds a variable to the factor graph and appends the scan as a bigdata element. +""" +function handleRadarPointcloud!(msg::sensor_msgs.msg.PointCloud2, fg::AbstractDFG, systemstate::SystemState) + @info "handleRadarPointcloud" maxlog=10 + + if systemstate.cur_variable === nothing + # Keyed by the radar, skip if we don't have a variable yet. + return nothing + end + timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/1.0e9 + @info "[$timestamp] RADAR pointcloud sample on $(systemstate.cur_variable.label)" + + # Make a data entry in the graph + ade,adb = addData!(fg, :radar, systemstate.cur_variable.label, :RADARPC, Vector{UInt8}(JSON2.write(msg)), mimeType="/radar_pointcloud_0;dataformat=Float32*[[X,Y,Z]]*12") +end + +function _handleRadarPointcloud!(msg, args::Tuple) + # sensor_msgs.msg.PointCloud2 + @info "_pointcloud" +end + +""" + $SIGNATURES + +Message callback for LIDAR point clouds. Adds a variable to the factor graph and appends the scan as a bigdata element. +Note that we're just appending all the LIDAR scans to the variables because we are keying by RADAR. +""" +function handleLidar!(msg::sensor_msgs.msg.PointCloud2, fg::AbstractDFG, systemstate::SystemState) + @info "handleLidar" maxlog=10 + # Compare systemstate and add the LIDAR scans if we want to. + if systemstate.cur_variable === nothing + return nothing + end + timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/1.0e9 + @info "[$timestamp] LIDAR pointcloud sample on $(systemstate.cur_variable.label) (sample $(systemstate.lidar_scan_index+1))" + + # Check if we have enough LIDAR's for this variable + if systemstate.lidar_scan_index >= systemstate.max_lidar + @warn "Ditching LIDAR sample for this variable, already have enough..." + return nothing + end + + # Make a data entry in the graph + ade,adb = addData!(fg, :lidar, systemstate.cur_variable.label, Symbol("LIDAR$(systemstate.lidar_scan_index)"), Vector{UInt8}(JSON2.write(msg)), mimeType="/velodyne_points;dataformat=Float32*[[X,Y,Z]]*32") + + # NOTE: If JSON, then do this to get to Vector{UInt8} - # byteData = Vector{UInt8}(JSON2.write(xyzLidarF32)) + + # Increment LIDAR scan count for this timestamp + systemstate.lidar_scan_index += 1 +end + +""" + $SIGNATURES + +Message callback for Radar pings. Adds a variable to the factor graph and appends the scan as a bigdata element. +""" +function handleGPS!(msg::sensor_msgs.msg.NavSatFix, fg::AbstractDFG, systemstate::SystemState) + @info "handleGPS" maxlog=10 + if systemstate.cur_variable === nothing + # Keyed by the radar, skip if we don't have a variable yet. + return nothing + end + timestamp = Float64(msg.header.stamp.secs) + Float64(msg.header.stamp.nsecs)/10^9 + # Update the variable if needed + # updateVariableIfNeeded(fg, systemstate, timestamp) + @info "[$timestamp] GPS sample on $(systemstate.cur_variable.label)" + + if :GPS in listDataEntries(fg, systemstate.cur_variable.label) + @warn "GPS sample on $(systemstate.cur_variable.label) already exist, dropping" + return nothing + end + + io = IOBuffer() + JSON2.write(io, msg) + ade,adb = addData!(fg, :gps_fix, systemstate.cur_variable.label, :GPS, take!(io), mimeType="application/json", description="JSON2.read(IOBuffer(datablob))") + +end + + +function _handleRadar!(msgdata, args::Tuple) + msgT = _unpackROSMsgType(seagrant_msgs.msg.radar, msgdata) + handleRadar!(msgT, args...) +end + +function _handleLidar!(msgdata, args::Tuple) + msgT = _unpackROSMsgType(sensor_msgs.msg.PointCloud2, msgdata) + handleLidar!(msgT, args...) +end + +function _handleGPS!(msgdata, args) + msgT = _unpackROSMsgType(sensor_msgs.msg.NavSatFix, msgdata) + handleGPS!(msgT, args...) +end + +function main(;iters::Integer=50) + dfg_datafolder = "/tmp/caesar/rex" + if isdir(dfg_datafolder) + rm(dfg_datafolder; force=true, recursive=true) + end + mkdir(dfg_datafolder) + + @info "Hit CTRL+C to exit and save the graph..." + + init_node("rex_feed") + # find the bagfile + bagfile = joinpath(ENV["HOME"],"data","Marine","basin_radar.bag") # lidar_radar.bag + bagSubscriber = RosbagSubscriber(bagfile) + + # Initialization + fg = initfg() + + ds = FolderStore{Vector{UInt8}}(:radar, "$dfg_datafolder/data/radar") + addBlobStore!(fg, ds) + + ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfg_datafolder/data/gps") + addBlobStore!(fg, ds) + + # add if you want lidar also + ds = FolderStore{Vector{UInt8}}(:lidar, "$dfg_datafolder/data/lidar") + addBlobStore!(fg, ds) + + # System state + systemstate = SystemState() + + + # Enable and disable as needed. + # Skipping LIDAR because those are huge... + # radar_sub = Subscriber{seagrant_msgs.msg.radar}("/radar_0", handleRadar!, (fg, systemstate), queue_size = 10) + radar_sub = bagSubscriber("/radar_0", _handleRadar!, (fg, systemstate)) + + + # radarpc_sub = Subscriber{sensor_msgs.msg.PointCloud2}("/radar_pointcloud0", handleRadarPointcloud!, (fg, systemstate), queue_size = 10) + # lidar_sub = Subscriber{sensor_msgs.msg.PointCloud2}("/velodyne_points", handleLidar!, (fg,systemstate), queue_size = 10) + # gps_sub = Subscriber{sensor_msgs.msg.NavSatFix}("/gps/fix", handleGPS!, (fg, systemstate), queue_size = 10) + gps_sub = bagSubscriber("/gps/fix", _handleGPS!, (fg, systemstate)) + + + @info "subscribers have been set up; entering main loop" + # loop_rate = Rate(20.0) + while loop!(bagSubscriber) + iters -= 1 + iters < 0 ? break : nothing + end + + @info "Exiting" + # After the graph is built, for now we'll save it to drive to share. + # Save the DFG graph with the following: + @info "Saving DFG to $dfg_datafolder/dfg" + saveDFG(fg, "$dfg_datafolder/dfg") + +end + +## + + +main(iters=100) + +# 275 + + +## after the graph is saved it can be loaded and the datastores retrieved + +dfg_datafolder = "/tmp/rex" + +fg = loadDFG("$dfg_datafolder/dfg") + +ds = FolderStore{Vector{UInt8}}(:radar, "$dfg_datafolder/data/radar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfg_datafolder/data/gps") +addBlobStore!(fg, ds) + +# add if you want lidar also +ds = FolderStore{Vector{UInt8}}(:lidar, "$dfg_datafolder/data/lidar") +addBlobStore!(fg, ds) \ No newline at end of file diff --git a/examples/marine/asv/rex/RExRadarProcessor.jl b/examples/marine/asv/rex/RExRadarProcessor.jl index cb304da16..b315d6f75 100644 --- a/examples/marine/asv/rex/RExRadarProcessor.jl +++ b/examples/marine/asv/rex/RExRadarProcessor.jl @@ -2,15 +2,17 @@ This script shows how to assemble full 360 degree radar sweeps from the DFG object built with RExFeed.jl """ - s + using DistributedFactorGraphs using IncrementalInference, RoME using JSON2 using LinearAlgebra +## + # Where to fetch data -dfgDataFolder = ENV["HOME"]*"/data/rex"; -# dfgDataFolder = "/tmp/rex" +# dfgDataFolder = ENV["HOME"]*"/data/rex"; +dfgDataFolder = "/tmp/caesar/rex" # Load the graph fg = loadDFG("$dfgDataFolder/dfg") @@ -25,20 +27,20 @@ addBlobStore!(fg, ds) ds = FolderStore{Vector{UInt8}}(:lidar, "$dfgDataFolder/data/lidar") addBlobStore!(fg, ds) - +## # Check what's in it ls(fg) lsf(fg) # there should be no factors the first time a session is loaded # How about radar data entries? -count(v -> :RADAR in listDataEntries(v), getVariables(fg)); +count(v -> :RADAR in listDataEntries(v), getVariables(fg)) # may be too intense as it is fetching ALL data too? allRadarVariables = filter(v -> :RADAR in listDataEntries(v), getVariables(fg)); +allRadarVariables = sortDFG(allRadarVariables); -allRadarVariables = sortDFG(allRadarVariables) function fetchRadarMsg(var::DFGVariable) entry,rawData = getData(fg, var.label, :RADAR) @@ -48,6 +50,8 @@ end msg = fetchRadarMsg(allRadarVariables[1]) +## + # function azimuth(msg::NamedTuple) function azimuth(msg) """ @@ -218,11 +222,12 @@ end # (polar, cart) = assembleping(msg, 2.0); # fullsweep .|= round.(UInt8, cart./255) # imshow!(imi["gui"]["canvas"], fullsweep) - # end # end # end +## + # set resolution to 1m/px (polar, cart) = assembleping(msg, 1.0); @@ -230,16 +235,18 @@ end using ImageMagick using Images, ImageView using ImageFiltering -imshow(polar) -imshow(cart) +# imshow(polar) +# imshow(cart) + +## output_dir = joinpath(dfgDataFolder,"pings") if (!isdir(output_dir)) mkdir(output_dir) end -save(joinpath(output_dir,"ping.jpg"),UInt8.(cart)) - +save(joinpath(output_dir,"ping.png"),UInt8.(cart)) +## # now let's process all radar variables in here fullsweep = zeros(size(cart)) @@ -270,7 +277,7 @@ for i in 1:length(allRadarVariables) # now that we have a full sweep, we can try to match with the previous one! # TODO: register w/ previous sweep # save to disk - fname = join(["full_",string(sweep),".jpg"]) + fname = join(["full_",string(sweep),".png"]) save(joinpath(output_dir,fname),UInt8.(clamp.(fullsweep,0,255))) # add full sweep to last variable @@ -280,10 +287,32 @@ for i in 1:length(allRadarVariables) fullsweep = zeros(size(cart)) sweep = sweep+1 end - # fname = join(["polar_",String(allRadarVariables[i].label),".jpg"]) + # fname = join(["polar_",String(allRadarVariables[i].label),".png"]) # save(joinpath(output_dir,fname),UInt8.(polar)) - fname = join(["cart_",String(allRadarVariables[i].label),".jpg"]) + fname = join(["cart_",String(allRadarVariables[i].label),".png"]) save(joinpath(output_dir,fname),UInt8.(cart)) last_angle = msg.angle_end end + + +## + + +# count(v -> :RADARSWEEP in listDataEntries(v), getVariables(fg)) + +# # may be too intense as it is fetching ALL data too? +# allSweepVariables = filter(v -> :RADARSWEEP in listDataEntries(v), getVariables(fg)); + +# allSweepVariables = sortDFG(allSweepVariables) + + +# getLabel.(allSweepVariables) + + +# de, db = getData(fg, allSweepVariables[1].label, :RADARSWEEP) + +# JSON2.read(IOBuffer(db)) + + +# \ No newline at end of file diff --git a/examples/marine/asv/rex/RadarFactor.jl b/examples/marine/asv/rex/RadarFactor.jl deleted file mode 100644 index 78242b8bb..000000000 --- a/examples/marine/asv/rex/RadarFactor.jl +++ /dev/null @@ -1,90 +0,0 @@ -""" -$TYPEDEF - -This is but one incarnation for how radar alignment factor could work, treat it as a starting point. - -Example -------- -```julia -using LinearAlgebra -arp2 = AlignRadarPose2(sweep[10], sweep[11], MvNormal(zeros(3), diagm([5;5;pi/4]))) -``` - -""" - -using TensorCast - -import IncrementalInference: getSample -import Base: convert - - -struct AlignRadarPose2{T <: Real, P <: SamplableBelief} <: FunctorPairwise - im1::Matrix{T} - im2::Matrix{T} - PreSampler::P - p2p2::Pose2Pose2 -end - -AlignRadarPose2(im1::Matrix{T}, im2::Matrix{T}, pres::P) where {T <: Real, P <: SamplableBelief} = AlignRadarPose2{T,P}(im1, im2, pres, Pose2Pose2(pres)) - -function getSample(rp2::AlignRadarPose2, N::Int=1) - - # closure on inner function - cost(x::AbstractVector) = evaluateTransform(rp2.im1,rp2.im2,x...) - - pres = rand(rp2.PreSampler, N) - out = zeros(3, N) - TASKS = Vector{Task}(undef, N) - for i in 1:N - # ignoring failures - TASKS[i] = Threads.@spawn optimize(cost, pres[:, $i], NelderMead()).minimizer - # out[:,i] = optimize(cost, pres[:,i], NelderMead()).minimizer - end - # retrieve threaded results - @sync for i in 1:N - @async out[:,$i] .= fetch(TASKS[$i]) - end - - # only using out, but return pres if user wants to look at it. - return (out, pres) -end - -function (rp2::AlignRadarPose2)(res::Vector{Float64}, - userdata::FactorMetadata, - idx::Int, - meas::Tuple, - wXi::Array{Float64,2}, - wXj::Array{Float64,2}) - # - rp2.p2p2(res, userdata, idx, meas, wXi, wXj) - res -end - -struct PackedAlignRadarPose2 <: PackedInferenceType - im1::Vector{Vector{Float64}} - im2::Vector{Vector{Float64}} - PreSampler::String - p2p2::PackedPose2Pose2 -end - -function convert(::Type{PackedAlignRadarPose2}, arp2::AlignRadarPose2) - TensorCast.@cast pim1[row][col] := arp2.im1[row,col] - TensorCast.@cast pim1[row] := collect(pim1[row]) - TensorCast.@cast pim2[row][col] := arp2.im2[row,col] - TensorCast.@cast pim2[row] := collect(pim2[row]) - PackedAlignRadarPose2( - pim1, - pim2, - string(arp2.PreSampler), - convert(PackedPose2Pose2, arp2.p2p2)) -end - -function convert(::Type{AlignRadarPose2}, parp2::PackedAlignRadarPose2) - TensorCast.@cast im1[row,col] := parp2.im1[row][col] - TensorCast.@cast im2[row,col] := parp2.im2[row][col] - AlignRadarPose2( - collect(im1), - collect(im2), - extractdistribution(parp2.PreSampler), - convert(Pose2Pose2, parp2.p2p2)) -end diff --git a/examples/marine/asv/rex/ScanMatcher.jl b/examples/marine/asv/rex/ScanMatcher.jl index f8f2aec9c..76d01a75d 100644 --- a/examples/marine/asv/rex/ScanMatcher.jl +++ b/examples/marine/asv/rex/ScanMatcher.jl @@ -2,191 +2,120 @@ this script fetches sequential pairs of poses, fetches the big data (radar pings) tied to those poses, and then determines the pairwise factors that should be added between these sequential pairs """ -using GraphPlot -using DistributedFactorGraphs -using IncrementalInference, RoME +using Images +using Caesar using JSON2 +using Manifolds + +using ImageView + +# import Rotations as _Rot + +# using Caesar: ScatterAlignPose2 + + +## # Where to fetch data -dfgDataFolder = joinpath(ENV["HOME"],"data","seagrant","rex") -datastore = FileDataStore("$dfgDataFolder/bigdata") +# dfgDataFolder = ENV["HOME"]*"/data/rex"; +dfgDataFolder = "/tmp/caesar/rex" + # Load the graph -fg = initfg() +fg = loadDFG("$dfgDataFolder/dfg") + +# add the datastore locations +ds = FolderStore{Vector{UInt8}}(:radar, "$dfgDataFolder/data/radar") +addBlobStore!(fg, ds) + +ds = FolderStore{Vector{UInt8}}(:gps_fix, "$dfgDataFolder/data/gps") +addBlobStore!(fg, ds) -# Reformat the data. -loadDFG!(fg, "$dfgDataFolder/fullsweep_updated.tar.gz"); +ds = FolderStore{Vector{UInt8}}(:lidar, "$dfgDataFolder/data/lidar") +addBlobStore!(fg, ds) + +## # fetch variables containing a full sweep -allSweepVariables = filter(v -> :RADARSWEEP in getBigDataKeys(v), getVariables(fg)) |> sortDFG +allSweepVariables = filter(v -> :RADARSWEEP in listDataEntries(v), getVariables(fg)) |> sortDFG + fsvars = allSweepVariables .|> getLabel # helper function to retrieve the radar sweep for a given variable -function fetchSweep(var::DFGVariable, store::FileDataStore) - entry = getBigDataEntry(var, :RADARSWEEP) - rawData = getBigData(datastore, entry) - # raw data is json-encoded; this decoding should happen inside getBigData? - rawdata = Vector{Float64}(JSON2.read(IOBuffer(rawData))) - n = Int(sqrt(length(rawdata))) - sweep = reshape(rawdata,(n,n)) - return sweep # That's pretty sweep if i say so myself... +function fetchSweep(dfg::AbstractDFG, varlabel::Symbol) + # + entry,rawData = getData(dfg, varlabel, :RADARSWEEP) + rawdata = Vector{Float64}(JSON2.read(IOBuffer(rawData))) + n = Int(sqrt(length(rawdata))) + sweep = reshape(rawdata,(n,n)) + return sweep # That's pretty sweep if i say so myself... end +## + # fetch all radar pings -sweeps = map(v -> fetchSweep(getVariable(fg, v), datastore), fsvars) -using Images, ImageView +sweeps = fetchSweep.(fg, fsvars); + # Filter the images kg = Kernel.gaussian(7) sweeps = map(s -> imfilter(s, kg), sweeps) # Normalize -sweeps = map(s -> s/maximum(s), sweeps) - -# Clamp out NaN's: -#map(clamp01nan, img) -# -# using ImageMagick -# for i in 5:length(sweeps) -# s = sweeps[i]./maximum(sweeps[i]) -# save("/home/gearsad/SlamInDb/seagrant/$i.jpg", Gray.(s)) -# end +sweeps = map(s -> s/maximum(s), sweeps); # At this point we can load the sweeps; let's work on registration # First step is to have a function that evaluates the cost of a given transform # between two subsequent images. -using CoordinateTransformations -using ImageTransformations - -# this function uses the sum of squared differences between the two images. -# To use low-passed versions of the images, simpy set the kernel argument to -# Kernel.gaussian(10) (or an appropriate size) -function getMismatch(a,b) - sqrt(sum((a.-b).^2)) -end -# sqrt(sum( imfilter( a.-b, Kernel.gaussian(5)).^2 )) -# sqrt(sum((imfilter(a, kernel).-imfilter(b, kernel)).^2)) - -# also do MMD version +## Building the graph -# Next step is to define a function that applies a transform to the image. This -# transform consists of a translation and a rotation -function transformImage(img::Array{Float64,2}, dx::Real, dy::Real, dh::Real) - tf = LinearMap(RotMatrix(dh))∘Translation(dx,dy) - tf_img = warp(img, tf) +# maybe one exists, or build a new one below +# newfg = loadDFG("$dfgDataFolder/newfg") - # replace NaN w/ 0 - mask = findall(x->isnan(x),tf_img) - tf_img[mask] .= 0.0 - return tf_img -end +startsweep = 5 +endsweep = 26 +graphinit = true +len = size(sweeps[5],1) +domain = (range(-100,100,length=976),range(-100,100,length=976)) -# now we can combine the two into an evaluation function -function evaluateTransform(a::Array{Float64,2},b::Array{Float64,2}, dx::Float64, dy::Float64, dh::Float64) - # transform image - bp = transformImage(b,dx,dy,dh) - # get matching padded views - ap, bpp = paddedviews(0.0, a, bp) - return getMismatch(ap,bpp) -end +## use a standard builder -## Building the graph -using DocStringExtensions -include("RadarFactor.jl") +STEP = 20 +sweeps_ = sweeps[startsweep:STEP:endsweep] -using LinearAlgebra -using Optim +# build the graph +newfg = buildGraphChain!( sweeps_, + ScatterAlignPose2, + (_, data) -> (data.currData,data.nextData,domain), + stopAfter=1, + doRef = false, + inflation_fct=0.0, + solverParams=SolverParams(graphinit=true, inflateCycles=1) ) +# -startsweep = 5 -endsweep = 10 -graphinit = false -# newfg = initfg() -newfg = generateCanonicalFG_ZeroPose(varType=Pose2) -for i in 1:(endsweep-startsweep) - addVariable!(newfg, Symbol("x$i"), Pose2, solvable=1) -end -for i in 1:(endsweep-startsweep) - factor = AlignRadarPose2(sweeps[i+startsweep-1], sweeps[i+startsweep], MvNormal(zeros(3), diagm([5;5;pi/4]))) - addFactor!(newfg, Symbol.(["x$(i-1)", "x$i"]), factor, graphinit=graphinit, solvable=1) -end -# Run the initialization (very slow right now) -# ensureAllInitialized!(newfg) +## -# Factor debugging -# fs = getFactorFunction.(getFactor.(newfg, lsf(newfg))) -# fs = filter(f -> f isa AlignRadarPose2, fs) -# pf = convert.(PackedAlignRadarPose3, fs) -# convert.(AlignRadarPose2, pf) # Save the graph -saveDFG(newfg, "$dfgDataFolder/segment_test.tar.gz"); +saveDFG("$dfgDataFolder/segment_test.tar.gz", newfg); + +## lsf(newfg) # this should run the radar alignment -pts = approxConv(newfg, :x0x1f1, :x1) +X1_ = approxConvBelief(newfg, :x0x1f1, :x1) +pts = getPoints(X1_) -# solving will internally call ensureAllInitialized!(newfg) +# solving will internally call initAll!(newfg) tree = solveTree!(newfg) -## Looking at the results -using Plots - -ppes = map(v -> getSuggestedPPE(getPPE(getVariable(newfg, v))), ls(newfg)) -x = map(ppe -> ppe[1], ppes); y = map(ppe -> ppe[2], ppes); h = map(ppe -> ppe[3], ppes) -Plots.plot(x, y, title="Path Plot", lw=3) - -## Stuff -using Optim +## +using Cairo, Gadfly +Gadfly.set_default_plot_size(40cm,20cm) -cost(x, im1, im2) = evaluateTransform(im1,im2,x[1],x[2],x[3]) - -# Plotting -xrange = -100.0:1.0:100.0 -hrange = -pi:0.1:pi -val = reshape( - [sweepx(sweeps[10],sweeps[11],xrange); sweepx(sweep_original[10],sweep_original[11],xrange)], - length(xrange), 2) -Plots.plot(xrange,val) -# Heading -val = reshape( - [sweeph(sweeps[10],sweeps[11],hrange); sweeph(sweep_original[10],sweep_original[11],hrange)], - length(hrange), 2) -Plots.plot(hrange,val) - -corr_func = (a,b)->sqrt(sum((a .- 0.5).*(b .- 0.5))) -val = reshape( - [sweepx(sweeps[10],sweeps[11],xrange,diff_func=corr_func); - sweepx(sweep_original[10],sweep_original[11],xrange,diff_func=corr_func)], - length(xrange), 2) -Plots.plot(xrange,val) - -## Sweep plotting -# sanity check: identity transform should yield zero cost -# @assert evaluateTransform(sweeps[11],sweeps[11],0.,0.,0.) == 0 "There's error with no transform!" - -# let's try small displacements: -# sweepx(im1, im2, xrange) = (x->@show evaluateTransform(im1,im2,x,0.,0.)).(xrange) -# sweepy(im1, im2, yrange) = (y->@show evaluateTransform(im1,im2,0.,y,0.)).(yrange) -# sweeph(im1, im2, hrange) = (h->@show evaluateTransform(im1,im2,0.,0.,h)).(hrange) - - -# using Plots -# xrange = -10:0.1:10 -# hrange = -pi:0.1:pi -# Plots.plot(xrange,sweepx(sweeps[10],sweeps[11],xrange)) -# Plots.plot(xrange,sweepy(sweeps[10],sweeps[11],xrange)) -# Plots.plot(hrange,sweeph(sweeps[10],sweeps[11],hrange)) - - -# fs10 = imfilter(sweeps[10],Kernel.gaussian(3)) -# fs11 = imfilter(sweeps[11],Kernel.gaussian(3)) -# ffs10 = imfilter(fs10,Kernel.gaussian(3)) -# ffs11 = imfilter(fs11,Kernel.gaussian(3)) -# -# Plots.plot(xrange,sweepx(ffs10,ffs11,xrange)) -# Plots.plot(xrange,sweepy(fs10,fs11,xrange)) +# \ No newline at end of file diff --git a/examples/marine/asv/rex/dev/ScanMatchAnalysis.jl b/examples/marine/asv/rex/dev/ScanMatchAnalysis.jl new file mode 100644 index 000000000..da4b731d8 --- /dev/null +++ b/examples/marine/asv/rex/dev/ScanMatchAnalysis.jl @@ -0,0 +1,62 @@ + + +## Looking at the results +using Plots + +ppes = map(v -> getPPE(getVariable(newfg, v)).suggested, ls(newfg)) +x = map(ppe -> ppe[1], ppes); y = map(ppe -> ppe[2], ppes); h = map(ppe -> ppe[3], ppes) +Plots.plot(x, y, title="Path Plot", lw=3) + + +## Stuff +using Optim + + +cost(tf, im1, im2) = evaluateTransform(im1,im2, tf ) + + +# Plotting +xrange = -100.0:1.0:100.0 +hrange = -pi:0.1:pi +val = reshape( + [sweepx(sweeps[10],sweeps[11],xrange); sweepx(sweep_original[10],sweep_original[11],xrange)], + length(xrange), 2) +Plots.plot(xrange,val) +# Heading +val = reshape( + [sweeph(sweeps[10],sweeps[11],hrange); sweeph(sweep_original[10],sweep_original[11],hrange)], + length(hrange), 2) +Plots.plot(hrange,val) + +corr_func = (a,b)->sqrt(sum((a .- 0.5).*(b .- 0.5))) +val = reshape( + [sweepx(sweeps[10],sweeps[11],xrange,diff_func=corr_func); + sweepx(sweep_original[10],sweep_original[11],xrange,diff_func=corr_func)], + length(xrange), 2) +Plots.plot(xrange,val) + +## Sweep plotting +# sanity check: identity transform should yield zero cost +# @assert evaluateTransform(sweeps[11],sweeps[11],0.,0.,0.) == 0 "There's error with no transform!" + +# let's try small displacements: +# sweepx(im1, im2, xrange) = (x->@show evaluateTransform(im1,im2,x,0.,0.)).(xrange) +# sweepy(im1, im2, yrange) = (y->@show evaluateTransform(im1,im2,0.,y,0.)).(yrange) +# sweeph(im1, im2, hrange) = (h->@show evaluateTransform(im1,im2,0.,0.,h)).(hrange) + + +# using Plots +# xrange = -10:0.1:10 +# hrange = -pi:0.1:pi +# Plots.plot(xrange,sweepx(sweeps[10],sweeps[11],xrange)) +# Plots.plot(xrange,sweepy(sweeps[10],sweeps[11],xrange)) +# Plots.plot(hrange,sweeph(sweeps[10],sweeps[11],hrange)) + + +# fs10 = imfilter(sweeps[10],Kernel.gaussian(3)) +# fs11 = imfilter(sweeps[11],Kernel.gaussian(3)) +# ffs10 = imfilter(fs10,Kernel.gaussian(3)) +# ffs11 = imfilter(fs11,Kernel.gaussian(3)) +# +# Plots.plot(xrange,sweepx(ffs10,ffs11,xrange)) +# Plots.plot(xrange,sweepy(fs10,fs11,xrange)) diff --git a/examples/marine/asv/rex/dev/ScatterSensitivityAnalysis.jl b/examples/marine/asv/rex/dev/ScatterSensitivityAnalysis.jl new file mode 100644 index 000000000..c3a8bbc52 --- /dev/null +++ b/examples/marine/asv/rex/dev/ScatterSensitivityAnalysis.jl @@ -0,0 +1,101 @@ + + +using Statistics +using DataFrames +using JLD2, FileIO + +using Cairo, Gadfly +Gadfly.set_default_plot_size(40cm,20cm) + +## plotting + +sap_ = getFactorType(newfg, :x0x1f1) + +## + +Caesar.plotScatterAlign(sap_, bw=0.001) + +## + +# snt_ = deepcopy(snt) +snt = overlayScatterMutate(sap_; user_offset=[-20.0;+100; pi/4], sample_count=200, bw=1e-4, user_coords=[0;-50;0.]) #, user_coords=snt_.best_coords) +plotScatterAlign(snt) + + + + +## sensitivity sweep on bw + + +SNT_10 = [[overlayScatterMutate(sap_; user_offset=[-20.0;+20;pi/4], sample_count=100, bw) for i in 1:200] for bw in exp10.(-6:1:-1)]; + +mkdir("/tmp/caesar/rex/SNT_10") +@save "/tmp/caesar/rex/SNT_10/SNT_10.jld2" SNT_10 + +## histogram sensitivities + +df = DataFrame(s=Int[], bw=Float64[], group=Symbol[], μ=Float64[], σ=Float64[], _μ=Float64[], μ_=Float64[] ) + +for (i,s) in enumerate(-6:-1) + + tl = "e$s" + X_ = (x->x.best_coords[1]).(SNT[i]) + Y_ = (x->x.best_coords[2]).(SNT[i]) + R_ = (x->x.best_coords[3]).(SNT[i]) + + μ_x = Statistics.mean(X_); σ_x = Statistics.std(X_) + μ_y = Statistics.mean(Y_); σ_y = Statistics.std(Y_) + μ_r = Statistics.mean(R_); σ_r = Statistics.std(R_) + + _μx, μx_ = μ_x - σ_x, μ_x + σ_x + _μy, μy_ = μ_y - σ_y, μ_y + σ_y + _μr, μr_ = μ_r - σ_r, μ_r + σ_r + + push!( df, ( s,exp10(s),:x, μ_x,σ_x, _μx,μx_ ) ) + push!( df, ( s,exp10(s),:y, μ_y,σ_y, _μy,μy_ ) ) + push!( df, ( s,exp10(s),:r, μ_r,σ_r, _μr,μr_ ) ) + + # Hx = Gadfly.plot(x=X_, Geom.histogram, Guide.title("X, $tl")) + # Hy = Gadfly.plot(x=Y_, Geom.histogram, Guide.title("Y, $tl")) + # Ht = Gadfly.plot(x=R_, Geom.histogram, Guide.title("θ, $tl")) + # hstack(Hx, Hy, Ht); +end + +## + +df_xy = filter( row -> row.group != :r, df ) +df_r = filter( row -> row.group == :r, df ) + +p_xy = Gadfly.plot(df_xy, x=:bw, y=:μ, ymin=:_μ, ymax=:μ_, color=:group, Geom.line, Geom.errorbar, Scale.x_log10) +p_r = Gadfly.plot(df_r, x=:bw, y=:μ, ymin=:_μ, ymax=:μ_, color=:group, Geom.line, Geom.errorbar, Scale.x_log10) + +hstack(p_xy, p_r) + + + +##============================================================================================= +## dev testing on one scatter image with known transform + +# sweeps_ = sweeps[5:6] +# sweeps_[2] = sweeps[5] + +# domain = (range(-100,100,length=976),range(-100,100,length=976)) + +# newfg = buildGraphChain!( sweeps_, +# ScatterAlignPose2, +# (_, data) -> (data.currData,data.nextData,domain), +# stopAfter=1, +# doRef = false, +# inflation_fct=0.0, +# solverParams=SolverParams(graphinit=false, inflateCycles=1) ) +# # + +## + +# fct = getFactorType(newfg, :x0x1f1) +# Xtup = sampleFactor(newfg, :x0x1f1) +# e0 = identity_element(SpecialEuclidean(2)) +# δ = calcFactorResidualTemporary(fct, (Pose2,Pose2), Xtup, (e0, e0)) + + +# \ No newline at end of file diff --git a/examples/marine/asv/rex/dev/VisualizeScanMatcher.jl b/examples/marine/asv/rex/dev/VisualizeScanMatcher.jl new file mode 100644 index 000000000..cae5687d6 --- /dev/null +++ b/examples/marine/asv/rex/dev/VisualizeScanMatcher.jl @@ -0,0 +1,49 @@ +# interactive scan match test + +using GLMakie +using Images, ImageTransformations +using ImageView + +## + +# arp = AlignRadarPose2(...) + +## + +fig = Figure() +ax = GLMakie.Axis(fig[1,1]) +guidict = ImageView.imshow(overlayScanMatcher(arp, [0;0], 0)) +canvas = guidict["gui"]["canvas"] + +guidict2 = imshow(arp.im1 - arp.im2) +canvas2 = guidict2["gui"]["canvas"] + + +sl_x = Slider(fig[2, 1], range = -40:1:40, startvalue = 0) +sl_r = Slider(fig[3, 1], range = (-pi/2):0.01:(pi/2), startvalue = 0) +sl_y = Slider(fig[1, 2], range = -40:1:40, horizontal = false, startvalue = 0) + +score = Ref(0.0) + +## + +lift(sl_x.value, sl_y.value, sl_r.value) do x, y, r + im_ = overlayScanMatcher(arp, [x;y], r; score) + println("Making x,y,r = $x,$y,$r = $(score[])") + ImageView.imshow(canvas, im_) + + a = arp.im1 + b = arp.im2 + bp = Caesar.transformImage_SE2(b, [x;y], r, arp.gridscale) + # get matching padded views + ap, bpp = paddedviews(0.0, a, bp) + # @show sqrt(sum((ap-bpp).^2)) + ImageView.imshow(canvas2, ap-bpp) +end + +fig + +## + + +# \ No newline at end of file diff --git a/examples/marine/rov/highend/lcmserver/pointclouddev.jl b/examples/marine/rov/highend/lcmserver/pointclouddev.jl index eee45e4bd..9b31271e0 100644 --- a/examples/marine/rov/highend/lcmserver/pointclouddev.jl +++ b/examples/marine/rov/highend/lcmserver/pointclouddev.jl @@ -31,7 +31,7 @@ function lcmpointcloudmsg!(vc, slaml::SLAMWrapper, # push to DrakeVisualizer #= position = Translation(msg[:pos]...) - orientation = Rotations.Quat(msg[:orientation]...) + orientation = _Rot.QuatRotation(msg[:orientation]...) Tf = position ∘ LinearMap(orientation) vsym = Symbol(vert.label) setgeometry!(vc[:submaps][vsym], Triad()) diff --git a/examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl b/examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl index e75061bb4..a305c925d 100644 --- a/examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl +++ b/examples/marine/rov/highend/lcmserver/resolvingPartialXYH.jl @@ -8,16 +8,16 @@ import DrakeVisualizer: Triad import Base: convert using Base: Test - +import Rotations as _Rot # random testing and converstion to be moved out function convert{T <: CoordinateTransformations.AffineMap}(::Type{T}, x::SE3) q = convert(TransformUtils.Quaternion, x.R) - Translation(x.t...) ∘ LinearMap( Rotations.Quat(q.s, q.v...) ) + Translation(x.t...) ∘ LinearMap( _Rot.QuatRotation(q.s, q.v...) ) end -function convert{T <: CoordinateTransformations.AffineMap{Rotations.Quat{Float64}}}(::Type{SE3}, x::T) +function convert{T <: CoordinateTransformations.AffineMap{_Rot.QuatRotation{Float64}}}(::Type{SE3}, x::T) SE3(x.v[1:3], TransformUtils.Quaternion(x.m.w, [x.m.x,x.m.y,x.m.z]) ) end diff --git a/examples/wheeled/racecar/cameraUtils.jl b/examples/wheeled/racecar/cameraUtils.jl index a4dee085a..91d5cb454 100644 --- a/examples/wheeled/racecar/cameraUtils.jl +++ b/examples/wheeled/racecar/cameraUtils.jl @@ -34,7 +34,7 @@ function rodrigues!(Rmat::Array{Float64,2}, rvec::Vector{Float64}) end function buildtagdict(cTt, - Ql::Rotations.Quat, + Ql::_Rot.QuatRotation, tvec, # ::Union{Vector{Float64},StaticArrays.SArray{Tuple{3},<:Real,1,3}} tagsize::Float64, bTcl ) @@ -56,7 +56,7 @@ function buildtagdict(cTt, onetag end # function buildtagdict(cTt, -# q::Rotations.Quat, +# q::_Rot.QuatRotation, # tvec::CoordinateTransformations.Translation{T}, # tagsize::Float64, # bTcl ) where T @@ -122,7 +122,7 @@ function getAprilTagTransform(tag::AprilTag; end -function prepTagBag(TAGS; iterposes::Int=99999999) +function prepTagBag(TAGS; iterposes::Int=(2^(Sys.WORD_SIZE-1)-1)) tvec = Translation(0.0,0,0) q = Quat(1.0,0,0,0) # package tag detections for all keyframes in a tag_bag @@ -149,7 +149,7 @@ end ## DETECT APRILTAGS FROM IMAGE DATA -function detectTagsInImgs(datafolder, imgfolder, resultsdir, camidxs; iterposes::Int=999999999) +function detectTagsInImgs(datafolder, imgfolder, resultsdir, camidxs; iterposes::Int=(2^(Sys.WORD_SIZE-1)-1)) # prep keyframe image data camlookup = prepCamLookup(camidxs) diff --git a/examples/wheeled/racecar/ros/CarSlamUtilsCommon.jl b/examples/wheeled/racecar/ros/CarSlamUtilsCommon.jl index f3caa32fc..32b7d752e 100644 --- a/examples/wheeled/racecar/ros/CarSlamUtilsCommon.jl +++ b/examples/wheeled/racecar/ros/CarSlamUtilsCommon.jl @@ -4,6 +4,8 @@ using LinearAlgebra using DataStructures using RoME, DistributedFactorGraphs +import Rotations as _Rot + ## clear old memory # delete!(vis[:tags]) diff --git a/examples/wheeled/racecar/ros/CarSlamUtilsMono.jl b/examples/wheeled/racecar/ros/CarSlamUtilsMono.jl index d3e2d9b34..aca944254 100644 --- a/examples/wheeled/racecar/ros/CarSlamUtilsMono.jl +++ b/examples/wheeled/racecar/ros/CarSlamUtilsMono.jl @@ -225,7 +225,7 @@ function updateSLAMMono!(fec::FrontEndContainer, # pose = homographytopose(tag.H, fx, fy, cx, cy, taglength = 160.) pose, err1 = tagOrthogonalIteration(tag, fx, fy, cx, cy, taglength = 0.25) cTt = LinearMap(pose[1:3, 1:3])∘Translation((pose[1:3,4])...) - bRc = Rotations.Quat(1/sqrt(2),0,0,-1/sqrt(2))*Rotations.Quat(1/sqrt(2),-1/sqrt(2),0,0) + bRc = _Rot.QuatRotation(1/sqrt(2),0,0,-1/sqrt(2))*_Rot.QuatRotation(1/sqrt(2),-1/sqrt(2),0,0) bTt = LinearMap(bRc) ∘ cTt # wTb = LinearMap(zT.R.R) ∘ Translation(zT.t...) # wTt = wTb ∘ bTt diff --git a/src/3rdParty/_PCL/_PCL.jl b/src/3rdParty/_PCL/_PCL.jl new file mode 100644 index 000000000..ab7364122 --- /dev/null +++ b/src/3rdParty/_PCL/_PCL.jl @@ -0,0 +1,37 @@ + +@info "Caesar.jl is loading (but not exporting) tools requiring Colors.jl; e.g. Caesar._PCL" + +module _PCL + +using ..Colors + +using Dates +using DocStringExtensions +using Requires +using StaticArrays +using Manifolds +import Rotations as _Rot + +# Going to add dispatches on these functions +import Base: getindex, setindex!, resize!, cat, convert, sizeof, hasproperty, getproperty + +# gets overloaded +import Manifolds: apply + +## hold off on exports, users can in the mean-time use/import via e.g. _PCL.PointXYZ +# export PointT, PointXYZ, PointXYZRGB, PointXYZRGBA +# export PCLHeader, PointCloud + + +# bring in the types +include("entities/PCLTypes.jl") +# bring in further source code +include("services/PointCloud.jl") + + +function __init__() + @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("services/ROSConversions.jl") +end + + +end \ No newline at end of file diff --git a/src/3rdParty/_PCL/entities/PCLTypes.jl b/src/3rdParty/_PCL/entities/PCLTypes.jl new file mode 100644 index 000000000..a2e7eabfa --- /dev/null +++ b/src/3rdParty/_PCL/entities/PCLTypes.jl @@ -0,0 +1,256 @@ +""" + Software License Agreement (BSD License) + + Point Cloud Library (PCL) - www.pointclouds.org + Copyright (c) 2010, Willow Garage, Inc. + Copyright (c) 2012-, Open Perception, Inc. + + All rights reserved. + + Redistribution and use in source and binary forms, with or without + modification, are permitted provided that the following conditions + are met: + + * Redistributions of source code must retain the above copyright + notice, this list of conditions and the following disclaimer. + * Redistributions in binary form must reproduce the above + copyright notice, this list of conditions and the following + disclaimer in the documentation and/or other materials provided + with the distribution. + * Neither the name of the copyright holder(s) nor the names of its + contributors may be used to endorse or promote products derived + from this software without specific prior written permission. + + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + POSSIBILITY OF SUCH DAMAGE. +""" + + +""" + _PCL_ENDIAN + +FIXME These values have not been validated! + +- `_PCL_ENDIAN_BIG_BYTE`, byte-swapped big-endian. +- `_PCL_ENDIAN_BIG_WORD`, word-swapped big-endian. +- `_PCL_ENDIAN_LITTLE_BYTE`, byte-swapped little-endian. +- `_PCL_ENDIAN_LITTLE_WORD`, word-swapped little-endian. + +Notes: +- https://docs.julialang.org/en/v1/base/io-network/#Base.ENDIAN_BOM +- https://www.boost.org/doc/libs/1_69_0/boost/predef/other/endian.h + +DevNotes +- FIXME, `_PCL_ENDIAN_BIG_WORD`, `_PCL_ENDIAN_LITTLE_BYTE` -- values are not correct +""" +@enum _PCL_ENDIAN begin + _PCL_ENDIAN_BIG_BYTE=0x01020304 + _PCL_ENDIAN_BIG_WORD=0x00020304 + _PCL_ENDIAN_LITTLE_BYTE=0x04030200 + _PCL_ENDIAN_LITTLE_WORD=0x04030201 +end + +Base.convert(::Type{<:_PCL_ENDIAN}, val::Integer) = (en=instances(_PCL_ENDIAN); en[findfirst(Int.(en) .== Int.(convert(UInt32, val)))]) + +""" +Format`:::UInt8` as enum for PointCloud2 messages between ROS and PCL. + +References +- https://docs.ros.org/en/api/sensor_msgs/html/msg/PointField.html +""" +@enum _PCL_POINTFIELD_FORMAT begin + _PCL_INT8 = UInt8(1) + _PCL_UINT8 = UInt8(2) + _PCL_INT16 = UInt8(3) + _PCL_UINT16 = UInt8(4) + _PCL_INT32 = UInt8(5) + _PCL_UINT32 = UInt8(6) + _PCL_FLOAT32 = UInt8(7) + _PCL_FLOAT64 = UInt8(8) +end + +Base.convert(::Type{<:_PCL_POINTFIELD_FORMAT}, val::Integer) = (en=instances(_PCL_POINTFIELD_FORMAT); en[findfirst(Int.(en) .== Int.(convert(UInt8, val)))]) + +struct asType{T} end + +(::Type{<:asType{_PCL_INT8}})() = Int8 +(::Type{<:asType{_PCL_UINT8}})() = UInt8 +(::Type{<:asType{_PCL_INT16}})() = Int16 +(::Type{<:asType{_PCL_UINT16}})() = UInt16 +(::Type{<:asType{_PCL_INT32}})() = Int32 +(::Type{<:asType{_PCL_UINT32}})() = UInt32 +(::Type{<:asType{_PCL_FLOAT32}})() = Float32 +(::Type{<:asType{_PCL_FLOAT64}})() = Float64 + + +abstract type PointT end + +""" + $TYPEDEF + +Immutable PointXYZ with color information. E.g. `PointXYZ{RGB}`, `PointXYZ{Gray}`, etc. + +Aliases +- `PointXYZRGB` +- `PointXYZRGBA` + +See +- https://pointclouds.org/documentation/structpcl_1_1_point_x_y_z.html +- https://pointclouds.org/documentation/point__types_8hpp_source.html +""" +Base.@kwdef struct PointXYZ{C <: Colorant, T <: Number} <: PointT + color::C = RGBA(1,1,1,1) + data::SVector{4,T} = SVector(0,0,0,1f0) +end + + +""" + $TYPEDEF + +Immutable Header. + +See https://pointclouds.org/documentation/structpcl_1_1_p_c_l_header.html +""" +Base.@kwdef struct Header + """ The sequence number """ + seq::UInt32 = UInt32(0) + """ A timestamp associated with the time when the data was acquired. + The value represents microseconds since 1970-01-01 00:00:00 (the UNIX epoch). """ + stamp::UInt64 = UInt64(0) + """ Coordinate frame ID. """ + frame_id::String = "" +end + +# https://pointclouds.org/documentation/structpcl_1_1_p_c_l_point_field.html +Base.@kwdef struct PointField + """ name of field """ + name::String + """ Offset from start of point struct """ + offset::UInt32 = UInt32(0) + """ Datatype enumeration, see _PCL_POINTFIELD_FORMAT """ + datatype::_PCL_POINTFIELD_FORMAT = convert(_PCL_POINTFIELD_FORMAT, 7) + """ How many elements in the field """ + count::UInt32 = UInt32(0) +end + +# https://docs.ros.org/en/hydro/api/pcl/html/structpcl_1_1detail_1_1FieldMapping.html +# https://pointclouds.org/documentation/common_2include_2pcl_2point__cloud_8h_source.html#l00072 +Base.@kwdef mutable struct FieldMapping + serialized_offset::UInt32 = UInt32(0) + struct_offset::UInt32 = UInt32(0) + size::UInt32 = UInt32(0) +end + +struct FieldMatches{T<:PointT} end + +# https://docs.ros.org/en/hydro/api/pcl/html/structpcl_1_1detail_1_1FieldAdder.html +Base.@kwdef struct FieldAdder + fields_::Vector{<:PointField} = Vector{PointField}() +end + +# https://docs.ros.org/en/hydro/api/pcl/html/point__cloud_8h_source.html#l00066 +# https://pointclouds.org/documentation/common_2include_2pcl_2point__cloud_8h_source.html#l00072 +const MsgFieldMap = Vector{FieldMapping} + +# https://docs.ros.org/en/hydro/api/pcl/html/conversions_8h_source.html#l00091 +Base.@kwdef struct FieldMapper{T<:PointT} + fields_::Vector{<:PointField} = Vector{PointField}() + map_::Vector{<:FieldMapping} = Vector{FieldMapping}() +end + +""" + $TYPEDEF + +Immutable point cloud type. Immutable for performance, computations are more +frequent and intensive than anticipated frequency of constructing new clouds. + +References: +- https://pointclouds.org/documentation/structpcl_1_1_p_c_l_point_cloud2.html +- https://pointclouds.org/documentation/classpcl_1_1_point_cloud.html +- https://pointclouds.org/documentation/common_2include_2pcl_2point__cloud_8h_source.html +""" +Base.@kwdef struct PCLPointCloud2 + """ the point cloud header """ + header::Header = Header() + """ the point cloud height (if organized as image structure). Specifies the height + of the point cloud dataset in the number of points. HEIGHT has two meanings: + - it can specify the height (total number of rows) of an organized point cloud dataset; + - it is set to 1 for unorganized datasets (thus used to check whether a dataset is organized or not). """ + height::UInt32 = UInt32(0) + """ the point cloud width (if organized as image structure). Specifies the width + of the point cloud dataset in the number of points. WIDTH has two meanings: + - it can specify the total number of points in the cloud (equal with POINTS see below) for unorganized datasets; + - it can specify the width (total number of points in a row) of an organized point cloud dataset. """ + width::UInt32 = UInt32(0) + """ field descriptions of data """ + fields::Vector{<:PointField}= Vector{PointField}() + """ `Vector` of `<:PointT` representing the point cloud """ + data::Vector{UInt8} = Vector{UInt8}() + """ WARNING, untested """ + is_bigendian::_PCL_ENDIAN= convert(_PCL_ENDIAN, Base.ENDIAN_BOM) + point_step::UInt32 = UInt32(0) + row_step::UInt32 = UInt32(0) + """ true if points are invalid (e.g., have NaN or Inf values in any of their floating point fields). """ + is_dense::UInt8 = UInt8(0) +end + + +""" + $TYPEDEF + +Convert a PCLPointCloud2 binary data blob into a `Caesar._PCL.PointCloud{T}` object using +a `field_map::Caesar._PCL.MsgFieldMap`. + +Use `PointCloud(::Caesar._PCL.PCLPointCloud2)` directly or create you +own `MsgFieldMap`: + +```julia +field_map = Caesar._PCL.createMapping(msg.fields, field_map) +``` + +Notes +- Tested on Radar data with height `z=constant` for all points -- i.e. 2D sweeping scan where `.height=1`. + +DevNotes +- TODO .PCLPointCloud2 convert not tested on regular 3D data from structured light or lidar yet, but current implementation should be close (or already working). + + +References +- https://pointclouds.org/documentation/classpcl_1_1_point_cloud.html +- (seems older) https://docs.ros.org/en/hydro/api/pcl/html/conversions_8h_source.html#l00123 +""" +Base.@kwdef struct PointCloud{T<:PointT,P,R} + """ the point cloud header """ + header::Header = Header() + """ `Vector` of `UInt8` representing the cloud points """ + points::Vector{T} = Vector{typeof(PointXYZ())}() # {PointXYZ{RGB{Colors.FixedPointNumbers.N0f8}, Float32}} + """ the point cloud width (if organized as image structure). Specifies the width + of the point cloud dataset in the number of points. WIDTH has two meanings: + - it can specify the total number of points in the cloud (equal with POINTS see below) for unorganized datasets; + - it can specify the width (total number of points in a row) of an organized point cloud dataset. """ + width::UInt32 = UInt32(0) + """ the point cloud height (if organized as image structure). Specifies the height + of the point cloud dataset in the number of points. HEIGHT has two meanings: + - it can specify the height (total number of rows) of an organized point cloud dataset; + - it is set to 1 for unorganized datasets (thus used to check whether a dataset is organized or not). """ + height::UInt32 = UInt32(0) + """ true if points are invalid (e.g., have NaN or Inf values in any of their floating point fields). """ + is_dense::Bool = false + """ Sensor acquisition pose (origin/translation), optional.""" + sensor_origin_::P = SA[0f0;0;0] + """ sensor acquisition pose (rotation), optional.""" + sensor_orientation_::R = SMatrix{3,3}(1f0,0,0,0,1,0,0,0,1) +end + + +# \ No newline at end of file diff --git a/src/3rdParty/_PCL/services/PointCloud.jl b/src/3rdParty/_PCL/services/PointCloud.jl new file mode 100644 index 000000000..5dd31e5a7 --- /dev/null +++ b/src/3rdParty/_PCL/services/PointCloud.jl @@ -0,0 +1,365 @@ + +Base.sizeof(pt::PointXYZ) = sizeof(pt.data) + +# Construct helpers nearest to PCL +PointXYZRGBA( x::Real=0, y::Real=0, z::Real=Float32(1); + r::Real=1,g::Real=1,b::Real=1, alpha::Real=1, + color::Colorant=RGBA(r,g,b,alpha), + pos=SA[x,y,z], data=SA[pos...,1] ) = PointXYZ(;color,data) +# +PointXYZRGB( x::Real=0, y::Real=0, z::Real=Float32(1); + r::Real=1,g::Real=1,b::Real=1, + color::Colorant=RGB(r,g,b), + pos=SA[x,y,z], data=SA[pos...,1] ) = PointXYZ(;color,data) +# + +## ============================================================================================== +## translating property names from upside-down C++ meta functions not clean Julian style yet, but +## can easily refactor once enough unit tests for conversions and use-cases exist here in Julia + +function Base.hasproperty(P::Type{<:PointXYZ}, f::Symbol) + # https://github.com/PointCloudLibrary/pcl/blob/35e03cec65fb3857c1d4062e4bf846d841fb98df/common/include/pcl/conversions.h#L126 + # https://github.com/PointCloudLibrary/pcl/blob/35e03cec65fb3857c1d4062e4bf846d841fb98df/common/include/pcl/for_each_type.h#L70-L86 + # TODO, missing colors when point has RGB + f in union(fieldnames(P), [:x;:y;:z]) +end + +function Base.getproperty(p::PointXYZ, f::Symbol) + if f == :x + return getfield(p, :data)[1] + elseif f == :y + return getfield(p, :data)[2] + elseif f == :z + return getfield(p, :data)[3] + elseif f == :r + return getfield(p, :color).r + elseif f == :g + return getfield(p, :color).g + elseif f == :b + return getfield(p, :color).b + elseif f == :alpha + return getfield(p, :color).alpha + elseif f == :data || f == :pos + return getfield(p, :data) + elseif f == :color || f == :rgb + return getfield(p, :color) + end + error("PointXYZ has no field $f") +end + +# Add a few basic dispatches +Base.getindex(pc::PCLPointCloud2, i) = pc.data[i] +Base.setindex!(pc::PCLPointCloud2, pt::PointT, idx) = (pc.data[idx] = pt) +Base.resize!(pc::PCLPointCloud2, s::Integer) = resize!(pc.data, s) + +# Add a few basic dispatches +Base.getindex(pc::PointCloud, i) = pc.points[i] +Base.setindex!(pc::PointCloud, pt::PointT, idx) = (pc.points[idx] = pt) +Base.resize!(pc::PointCloud, s::Integer) = resize!(pc.points, s) + +## not very Julian translations from C++ above +## ============================================================================================== + + +# builds a new immutable object, reuse=true will modify and reuse parts of A and B +function Base.cat(A::PointCloud, B::PointCloud; reuse::Bool=false, stamp_earliest::Bool=true) + pc = PointCloud(; + header = Header(; + seq = A.header.seq, + stamp = stamp_earliest ? A.header.stamp : maximum(A.header.stamp, B.header.stamp), + frame_id = A.header.frame_id + ), + # can go a little faster, but modifies A + points = reuse ? A.points : deepcopy(A.points), + height = A.height, + width = A.width + B.width, + is_dense = A.is_dense && B.is_dense + ) + lenA = length(A.points) + lenB = length(B.points) + resize!(pc.points, lenA+lenB) + pc.points[(lenA+1):end] .= B.points + + # return the new PCLPointCloud2 object + return pc +end + +## + +function (fm::FieldMatches{PointXYZ{C,T}})(field::PointField) where {C,T} + # https://github.com/PointCloudLibrary/pcl/blob/35e03cec65fb3857c1d4062e4bf846d841fb98df/common/include/pcl/PCLPointField.h#L57 + # TODO complete all tests + hasproperty(PointXYZ{C,T}, Symbol(field.name)) && + asType{field.datatype}() == T && + (field.count == 1 || # FIXME, SHOULD NOT HARD CODE 1 + field.count == 0 && false) #.size == 1) + # ((field.count == traits::datatype::size) || + # (field.count == 0 && traits::datatype::size == 1 /* see bug #821 */))); +end + +# https://docs.ros.org/en/hydro/api/pcl/html/conversions_8h_source.html#l00091 +# https://github.com/PointCloudLibrary/pcl/blob/903f8b30065866ae5ca57f4c3606437476b51fcc/common/include/pcl/point_traits.h +function (fm!::FieldMapper{T})() where T + for field in fm!.fields_ + if FieldMatches{T}()(field) + mapping = FieldMapping(; + serialized_offset = field.offset, + struct_offset = field.offset, # FIXME which offset value to use here ??? + size = sizeof(asType{field.datatype}()) + ) + push!(fm!.map_, mapping) + # return nothing + end + end + if 0 < length(fm!.map_) + return nothing + end + @warn "Failed to find match for field..." +end + + +""" + $SIGNATURES + +Still incomplete, basic 2D and 3D *should* work. + +DevNotes +- Resolve, if necessary, conversions endianness with `htol`, `ltoh`, etc. + +Notes +- https://docs.ros.org/en/hydro/api/pcl/html/conversions_8h_source.html#l00115 +- fieldOrdering(a::FieldMapping, b::FieldMapping) = a.serialized_offset < b.serialized_offset +- https://docs.ros.org/en/hydro/api/pcl/html/conversions_8h_source.html#l00123 +- https://docs.ros.org/en/jade/api/pcl_conversions/html/namespacepcl.html +""" +function createMapping(T,msg_fields::AbstractVector{<:PointField}, field_map::MsgFieldMap=MsgFieldMap()) + # Create initial 1-1 mapping between serialized data segments and struct fields + mapper! = FieldMapper{T}(;fields_=msg_fields, map_=field_map) + # the idea here is that for_each_type<> will recursively call the operator `mapper()` on each element on msg_fields + # and then check if the desired destination PointCloud{<:PointT}'s points have field names that can absorb + # the data contained in msg_fields, e.g. `.name==:x`, etc as checked by `FieldMatcher`. This should + # add all msg_fields that match properties of destination cloud <:PointT into `field_map`. + # https://github.com/PointCloudLibrary/pcl/blob/35e03cec65fb3857c1d4062e4bf846d841fb98df/common/include/pcl/conversions.h#L126 + # https://github.com/PointCloudLibrary/pcl/blob/35e03cec65fb3857c1d4062e4bf846d841fb98df/common/include/pcl/for_each_type.h#L70-L86 + # 00127 for_each_type< typename traits::fieldList::type > (mapper); + mapper!() + + # Coalesce adjacent fields into single copy where possible + if 1 < length(field_map) + # TODO check accending vs descending order + sort!(field_map, by = x->x.serialized_offset) + + # something strange with how C++ does and skips the while loop, disabling the coalescing for now + # i = 1 + # j = i + 1 + # # TODO consolidate strips of memory into a single field_map -- e.g. [x,y,z],offsets=0,size=4 becomes, [x],offsets=0,size=12 + # _jend = field_map[end] + # _jmap = field_map[j] + # while _jmap != _jend + # # This check is designed to permit padding between adjacent fields. + # if (_jmap.serialized_offset - field_map[i].serialized_offset) == (_jmap.struct_offset - field_map[i].struct_offset) + # field_map[i].size += (_jmap.struct_offset + _jmap.size) - (field_map[i].struct_offset + field_map[i].size) + # @info "deleteat j" j + # # https://www.cplusplus.com/reference/vector/vector/erase/ + # deleteat!(field_map,j) + # _jmap = field_map[j] # same iterator j now points to shifted element in vector after deletion (still the same position after i). + # _jend = field_map[end] + # else + # i += 1 + # j += 1 + + # _jmap = field_map[j] + # _jend = field_map[end] + # end + # end + # @info "after coalesce" field_map + end + + return field_map +end + + +# https://pointclouds.org/documentation/conversions_8h_source.html#l00166 +function PointCloud( + msg::PCLPointCloud2, + cloud::PointCloud{T} = PointCloud(; + header = msg.header, + width = msg.width, + height = msg.height, + is_dense = msg.is_dense == 1 ), + field_map::MsgFieldMap=createMapping(T,msg.fields) + ) where {T} + # + cloudsize = msg.width*msg.height + # cloud_data = Vector{UInt8}(undef, cloudsize) + + # NOTE assume all fields use the same data type + # off script conversion for XYZ_ data only + datatype = asType{msg.fields[1].datatype}() + len = trunc(Int, length(msg.data)/field_map[1].size) + data_ = Vector{datatype}(undef, len) + read!(IOBuffer(msg.data), data_) + mat = reshape(data_, :, cloudsize) + + + # Check if we can copy adjacent points in a single memcpy. We can do so if there + # is exactly one field to copy and it is the same size as the source and destination + # point types. + if (length(field_map) == 1 && + field_map[1].serialized_offset == 0 && + field_map[1].struct_offset == 0 && + field_map[1].size == msg.point_step && + field_map[1].size == sizeof(T)) + # + error("copy of just one field_map not implemented yet") + else + # If not, memcpy each group of contiguous fields separately + @assert msg.height == 1 "only decoding msg.height=1 messages, update converter here." + for row in 1:msg.height + # TODO check might have an off by one error here + # row_data = row * msg.row_step + 1 # msg.data[(row-1) * msg.row_step] + for col in 1:msg.width + # msg_data = row_data + col*msg.point_step + # the slow way of building the point.data entry + ptdata = zeros(datatype, 4) + for (i,mapping) in enumerate(field_map) + midx = trunc(Int,mapping.serialized_offset/mapping.size) + 1 + # TODO, why the weird index reversal? + ptdata[i] = mat[midx, col] + # @info "DO COPY" mapping + # memcpy (cloud_data + mapping.struct_offset, msg_data + mapping.serialized_offset, mapping.size); + # @info "copy" mapping.struct_offset mapping.serialized_offset mapping.size + end + pt = T(;data=SVector(ptdata...)) + push!(cloud.points, pt) + # cloudsize += sizeof(T) + end + end + end + + return cloud +end + + +## ========================================================================================================= +## Coordinate transformations using Manifolds.jl +## ========================================================================================================= + + +# 2D, do similar or better for 3D +# FIXME, to optimize, this function will likely be slow +function apply( M_::typeof(SpecialEuclidean(2)), + rPp::Union{<:ProductRepr,<:Manifolds.ArrayPartition}, + pc::PointCloud{T} ) where T + # + + rTp = affine_matrix(M_, rPp) + pV = MVector(0.0,0.0,1.0) + _data = MVector(0.0,0.0,0.0,0.0) + + _pc = PointCloud(;header=pc.header, + points = Vector{T}(), + width=pc.width, + height=pc.height, + is_dense=pc.is_dense, + sensor_origin_=pc.sensor_origin_, + sensor_orientation_=pc.sensor_orientation_ ) + # + + # rotate the elements from the old point cloud into new static memory locations + # NOTE these types must match the types use for PointCloud and PointXYZ + for pt in pc.points + pV[1] = pt.x + pV[2] = pt.y + _data[1:3] .= rTp*pV + push!(_pc.points, PointXYZ(;color=pt.color, data=SVector{4,eltype(pt.data)}(_data[1], _data[2], pt.data[3:4]...)) ) + end + + # return the new point cloud + return _pc +end + + +## ========================================================================================================= +## Custom printing +## ========================================================================================================= + + +function Base.show(io::IO, hdr::Header) # where {T} + printstyled(io, "Caesar._PCL.Header", bold=true, color=:blue) + println(io) + println(io, " seq: ", hdr.seq) + println(io, " stamp*: ", unix2datetime(hdr.stamp*1e-6)) + println(io, " frame_id: ", hdr.frame_id) + + nothing +end + +Base.show(io::IO, ::MIME"text/plain", pc::Header) = show(io, pc) +Base.show(io::IO, ::MIME"application/prs.juno.inline", pc::Header) = show(io, pc) + + +function Base.show(io::IO, pc::PCLPointCloud2) + printstyled(io, "Caesar._PCL.PCLPointCloud2", bold=true, color=:blue) + # println(io) + # printstyled(io, " T = ", bold=true, color=:magenta) + # println(io, T) + # printstyled(io, " }", bold=true, color=:blue) + println(io) + print(io, " header::", pc.header) + println(io, " height: ", pc.height) + println(io, " width: ", pc.width) + print(io, " # fields: ", length(pc.fields)) + if 0 < length(pc.fields) + print(io, ": [") + for fld in pc.fields + print(io, fld.name, ",") + end + print(io, "]") + end + println(io) + println(io, " # data[]: ", length(pc.data) ) + println(io, " is_bigendian: ", pc.is_bigendian) + println(io, " point_step: ", pc.point_step ) + println(io, " row_step: ", pc.row_step ) + println(io, " is_dense: ", pc.is_dense ) + println(io) + nothing +end + +Base.show(io::IO, ::MIME"text/plain", pc::PCLPointCloud2) = show(io, pc) +Base.show(io::IO, ::MIME"application/prs.juno.inline", pc::PCLPointCloud2) = show(io, pc) + + + +function Base.show(io::IO, pc::PointCloud{T,P,R}) where {T,P,R} + printstyled(io, "Caesar._PCL.PointCloud{", bold=true, color=:blue) + println(io) + printstyled(io, " T = ", bold=true, color=:magenta) + println(io, T) + printstyled(io, " P = ", bold=true, color=:magenta) + println(io, P) + printstyled(io, " R = ", bold=true, color=:magenta) + println(io, R) + printstyled(io, " }", bold=true, color=:blue) + println(io) + println(io, " header: ", pc.header) + println(io, " width: ", pc.width) + println(io, " height: ", pc.height) + println(io, " points[::T]: ", length(pc.points) ) + println(io, " is_dense: ", pc.is_dense) + println(io, " sensor pose:") + println(io, " xyz: ", round.(pc.sensor_origin_, digits=3)) + q = convert(_Rot.QuatRotation, pc.sensor_orientation_) + println(io, " w_xyz*: ", round.(_Rot.params(q), digits=3)) + + nothing +end + +Base.show(io::IO, ::MIME"text/plain", pc::PointCloud) = show(io, pc) +Base.show(io::IO, ::MIME"application/prs.juno.inline", pc::PointCloud) = show(io, pc) + + + + +# \ No newline at end of file diff --git a/src/3rdParty/_PCL/services/ROSConversions.jl b/src/3rdParty/_PCL/services/ROSConversions.jl new file mode 100644 index 000000000..d37e7388f --- /dev/null +++ b/src/3rdParty/_PCL/services/ROSConversions.jl @@ -0,0 +1,38 @@ +# conversions between ROS and _PCL + +@info "Caesar._PCL is loading tools related to RobotOS.jl." +@debug "Caesar._PCL RobotOS.jl requires user to first run as @rostypegen (on messages below) in `Main` before loading Caesar." +@debug " @rosimport sensor_msgs.msg: PointCloud2" + +using ..RobotOS + +@rosimport sensor_msgs.msg: PointCloud2 + + +Base.convert(::Type{UInt64}, rost::RobotOS.Time) = UInt64(rost.secs)*1000000 + trunc(UInt64, rost.nsecs*1e-3) + +# Really odd constructor, strong assumption that user FIRST ran @rostypegen in Main BEFORE loading Caesar +# https://docs.ros.org/en/hydro/api/pcl_conversions/html/pcl__conversions_8h_source.html#l00208 +function PCLPointCloud2(msg::Main.sensor_msgs.msg.PointCloud2) + header = Header(;stamp = msg.header.stamp, + seq = msg.header.seq, + frame_id= msg.header.frame_id ) + # + + # all PointField elements + pfs = PointField[PointField(;name=pf_.name,offset=pf_.offset,datatype=pf_.datatype,count=pf_.count) for pf_ in msg.fields] + + endian = msg.is_bigendian ? _PCL_ENDIAN_BIG_BYTE : _PCL_ENDIAN_LITTLE_WORD + pc2 = PCLPointCloud2(;header, + height = msg.height, + width = msg.width, + fields = pfs, + data = msg.data, + is_bigendian= endian, + point_step = msg.point_step, + row_step = msg.row_step, + is_dense = UInt8(msg.is_dense) ) + # +end + +# \ No newline at end of file diff --git a/src/Caesar.jl b/src/Caesar.jl index f86598cc4..060bc7bb6 100644 --- a/src/Caesar.jl +++ b/src/Caesar.jl @@ -6,18 +6,19 @@ import RoME: getRangeKDEMax2D import IncrementalInference: getSample, initfg import DistributedFactorGraphs: getManifold -using Reexport +# handy project consts (not for export) +import IncrementalInference: NothingUnion, InstanceType + using Requires using Dates using Manifolds +using StaticArrays -@reexport using RoME -@reexport using IncrementalInference -@reexport using KernelDensityEstimate -@reexport using Distributions +import Rotations as _Rot -import Rotations as _Rotations +# TODO remove +const _Rotations = _Rot using Pkg, @@ -42,54 +43,16 @@ using TimeZones, TensorCast -export - GenericInSituSystem, # insitu components - makeGenericInSituSys, - InSituSystem, - makeInSituSys, - triggerPose, - poseTrigAndAdd!, - processTreeTrackersUpdates!, - advOdoByRules, - SLAMWrapper, - - # servers - tcpStringSLAMServer, - tcpStringBRTrackingServer, - - # user functions - identitypose6fg, - projectrbe, - hasval, - - # Robot Utils - getRangeKDEMax2D, - - # sas-slam - CBFFilterConfig, - CZTFilter, - prepCZTFilter, - getCBFFilter2Dsize, - constructCBFFilter2D!, - CBF2D_DelaySum!, - MatchedFilter, - SASBearing2D, - PackedSASBearing2D, - compare, - SASDebug, - reset!, - prepMF, - loadConfigFile, - prepareSAS2DFactor, - wrapRad, - phaseShiftSingle!, - liebf!, - SASDebug - - - -const NothingUnion{T} = Union{Nothing, T} +using Optim + +using Reexport + +# public API exports +include("ExportAPI.jl") + +## =============================================================================================== +# and source files include("BearingRangeTrackingServer.jl") include("SlamServer.jl") @@ -99,10 +62,8 @@ include("UserFunctions.jl") # Configuration include("config/CaesarConfig.jl") - include("Deprecated.jl") - # Multisession operation # include("attic/multisession/Multisession.jl") @@ -122,12 +83,21 @@ function __init__() @eval using .PyCall @require RobotOS="22415677-39a4-5241-a37a-00beabbbdae8" include("ros/Utils/RosbagSubscriber.jl") end + @require Colors="5ae59095-9a9b-59fe-a467-6f913c188581" include("3rdParty/_PCL/_PCL.jl") @require AprilTags="f0fec3d5-a81e-5a6a-8c28-d2b34f3659de" begin include("images/apriltags.jl") @require ImageDraw="4381153b-2b60-58ae-a1ba-fd683676385f" include("images/AprilTagDrawingTools.jl") end + @require ImageDraw="4381153b-2b60-58ae-a1ba-fd683676385f" include("images/imagedraw.jl") @require ImageMagick="6218d12a-5da1-5696-b52f-db25d2ecc6d1" include("images/imagedata.jl") - @require Images="916415d5-f1e6-5110-898d-aaa5f9f070e0" include("images/images.jl") + @require Images="916415d5-f1e6-5110-898d-aaa5f9f070e0" begin + include("images/images.jl") + include("images/ScanMatcherUtils.jl") + include("images/ScanMatcherPose2.jl") + include("images/ScatterAlignPose2.jl") + + @require Gadfly="c91e804a-d5a3-530f-b6f0-dfbca275c004" include("plotting/ScatterAlignPlotting.jl") + end @require Distributed="8ba89e20-285c-5b6f-9357-94700520ee1b" include("images/DistributedUtils.jl") end diff --git a/src/ExportAPI.jl b/src/ExportAPI.jl new file mode 100644 index 000000000..16d975668 --- /dev/null +++ b/src/ExportAPI.jl @@ -0,0 +1,49 @@ + +@reexport using RoME +@reexport using IncrementalInference +@reexport using KernelDensityEstimate +@reexport using Distributions + +export + GenericInSituSystem, # insitu components + makeGenericInSituSys, + InSituSystem, + makeInSituSys, + triggerPose, + poseTrigAndAdd!, + processTreeTrackersUpdates!, + advOdoByRules, + SLAMWrapper, + + # servers + tcpStringSLAMServer, + tcpStringBRTrackingServer, + + # user functions + identitypose6fg, + projectrbe, + hasval, + + # Robot Utils + getRangeKDEMax2D, + + # sas-slam + CBFFilterConfig, + CZTFilter, + prepCZTFilter, + getCBFFilter2Dsize, + constructCBFFilter2D!, + CBF2D_DelaySum!, + MatchedFilter, + SASBearing2D, + PackedSASBearing2D, + compare, + SASDebug, + reset!, + prepMF, + loadConfigFile, + prepareSAS2DFactor, + wrapRad, + phaseShiftSingle!, + liebf!, + SASDebug diff --git a/src/images/Pose2AprilTag4Corners.jl b/src/images/Pose2AprilTag4Corners.jl index 19d578bea..bd23d6a2e 100644 --- a/src/images/Pose2AprilTag4Corners.jl +++ b/src/images/Pose2AprilTag4Corners.jl @@ -135,7 +135,7 @@ function _AprilTagToPose2(corners, pose, err1 = AprilTags.tagOrthogonalIteration(corners, homography, f_width, f_height, c_width, c_height, taglength=taglength_) cVt = Translation((pose[1:3,4])...) # bRc = bRy * yRc - bRc = _Rotations.Quat(1/sqrt(2),0,1/sqrt(2),0) * _Rotations.Quat(1/sqrt(2),0,0,-1/sqrt(2)) + bRc = _Rot.QuatRotation(1/sqrt(2),0,1/sqrt(2),0) * _Rot.QuatRotation(1/sqrt(2),0,0,-1/sqrt(2)) # for tag in body frame == bTt bTt = LinearMap(bRc) ∘ cVt diff --git a/src/images/ScanMatcherPose2.jl b/src/images/ScanMatcherPose2.jl new file mode 100644 index 000000000..ea020a16b --- /dev/null +++ b/src/images/ScanMatcherPose2.jl @@ -0,0 +1,163 @@ + + +import IncrementalInference: getSample +import Base: convert, show +# import DistributedFactorGraphs: getManifold + +using .Images + +export ScanMatcherPose2, PackedScanMatcherPose2 + +""" +$TYPEDEF + +This is but one incarnation for how radar alignment factor could work, treat it as a starting point. + +Notes +- Stanard `cvt` argument is lambda function to convert incoming images to user convention of image axes, + - default `cvt` flips image rows so that Pose2 xy-axes corresponds to img[x,y] -- i.e. rows down and across from top left corner. +- Use rescale to resize the incoming images for lower resolution (faster) correlations +- Both images passed to the construct must have the same type some matrix of type `T`. + +Example +------- +```julia +arp2 = ScanMatcherPose2(img1, img2, 2) # e.g. 2 meters/pixel +``` + +See also: [`overlayScanMatcher`](@ref) +""" +struct ScanMatcherPose2{T} <: IIF.AbstractManifoldMinimize + """ reference image for scan matching. """ + im1::Matrix{T} + """ test image to scan match against the reference image. """ + im2::Matrix{T} + """ Common grid scale for both images -- i.e. units/pixel. + Constructor uses two arguments `gridlength`*`rescale=1`=`gridscale`. + Arg 0 < `rescale` ≤ 1 is also used to rescale the images to lower resolution for speed. """ + gridscale::Float64 + + # replace inner constructor with transform on image + ScanMatcherPose2{T}( im1::AbstractMatrix{T}, + im2::AbstractMatrix{T}, + gridlength::Real, + rescale::Real=1, + cvt = (im)->reverse(Images.imresize(im,trunc.(Int, rescale.*size(im))),dims=1) + ) where {T} = new{T}( cvt(im1), + cvt(im2), + rescale*gridlength ) +end + +ScanMatcherPose2(im1::AbstractMatrix{T}, im2::AbstractMatrix{T}, w...) where T = ScanMatcherPose2{T}(im1,im2, w...) + +getManifold(::IIF.InstanceType{<:ScanMatcherPose2}) = getManifold(Pose2Pose2) + +getSample( cf::CalcFactor{<:ScanMatcherPose2} ) = nothing + +function (cf::CalcFactor{<:ScanMatcherPose2})(X, p, q) + M = getManifold(Pose2) + arp = cf.factor + # for groups + tf = Manifolds.compose(M, inv(M, p), q) + Mr = M.manifold[2] + r0 = identity_element(Mr) + r = vee(Mr, r0, log(Mr, r0, tf.parts[2]))[1] + return evaluateTransform(arp.im1, arp.im2, tf.parts[1], r, arp.gridscale) +end + +function Base.show(io::IO, arp::ScanMatcherPose2{T}) where {T} + printstyled(io, "ScanMatcherPose2{", bold=true, color=:blue) + println(io) + printstyled(io, " T = ", color=:magenta) + println(io, T) + printstyled(io, " }", color=:blue, bold=true) + println(io) + println(io, " size(.im1): ", size(arp.im1)) + println(io, " min/max: ", round(minimum(arp.im1),digits=3), "/", round(maximum(arp.im1),digits=3)) + println(io, " size(.im2): ", size(arp.im2)) + println(io, " min/max: ", round(minimum(arp.im2),digits=3), "/", round(maximum(arp.im2),digits=3)) + println(io, " gridscale: ", arp.gridscale) + nothing +end + +Base.show(io::IO, ::MIME"text/plain", arp::ScanMatcherPose2) = show(io, arp) +Base.show(io::IO, ::MIME"application/juno.inline", arp::ScanMatcherPose2) = show(io, arp) + + +""" + $SIGNATURES + +Overlay the two images from `AlignRadarPose2` with the first (red) fixed and transform the second image (blue) according to `tf`. + +Notes: +- `tf` is a Manifolds.jl type `::ProductRepr` (or newer `::ArrayPartition`) to represent a `SpecialEuclidean(2)` manifold point. +""" +function overlayScanMatcher(sm::ScanMatcherPose2, + trans::AbstractVector{<:Real}=Float64[0;0], + rot::Real=0.0; + score=Ref(0.0), + showscore::Bool=true ) + # + im2_ = transformImage_SE2(sm.im2, trans, rot, sm.gridscale) + + # get matching padded views + im1_, im2__ = paddedviews(0.0, sm.im1, im2_) + + im1__ = RGBA.(im1_, 0, 0, 0.5) + im2___ = RGBA.(0, 0, im2__, 0.5) + + if showscore + score[] = evaluateTransform(sm.im1, sm.im2, trans, rot, sm.gridscale) + @info "overlayScanMatcher score" score[] + end + + # im2__ = RGBA.(im2_, 0, 0, 0.5) + im1__ .+ im2___ +end + + +# + +function overlayScanMatcher(sm::ScanMatcherPose2, + tf = Manifolds.identity_element(SpecialEuclidean(2)); + kw... ) + # + M = SpecialOrthogonal(2) + e0 = identity_element(M) + + rot = vee( M, e0, log(M, e0, tf.parts[2]) )[1] + + overlayScanMatcher(sm, tf.parts[1], rot; kw...) +end + +## ========================================================================================= +## Factor serialization below +## ========================================================================================= + +struct PackedScanMatcherPose2 <: PackedInferenceType + im1::Vector{Vector{Float64}} + im2::Vector{Vector{Float64}} + gridscale::Float64 +end + +function convert(::Type{<:PackedScanMatcherPose2}, arp2::ScanMatcherPose2) + TensorCast.@cast pim1[row][col] := arp2.im1[row,col] + TensorCast.@cast pim1[row] := collect(pim1[row]) + TensorCast.@cast pim2[row][col] := arp2.im2[row,col] + TensorCast.@cast pim2[row] := collect(pim2[row]) + PackedScanMatcherPose2( + pim1, + pim2, + arp2.gridscale ) +end + +function convert(::Type{<:ScanMatcherPose2}, parp2::PackedScanMatcherPose2) + TensorCast.@cast im1[row,col] := parp2.im1[row][col] + TensorCast.@cast im2[row,col] := parp2.im2[row][col] + ScanMatcherPose2( + collect(im1), + collect(im2), + parp2.gridscale ) +end + +# \ No newline at end of file diff --git a/src/images/ScanMatcherUtils.jl b/src/images/ScanMatcherUtils.jl new file mode 100644 index 000000000..18510a66d --- /dev/null +++ b/src/images/ScanMatcherUtils.jl @@ -0,0 +1,52 @@ +# Utilities for scan matching + +export overlayScanMatcher + + +# this function uses the sum of squared differences between the two images. +# To use low-passed versions of the images, simpy set the kernel argument to +# Kernel.gaussian(10) (or an appropriate size) +getMismatch(a,b) = sqrt(sum((a-b).^2)) + +# sqrt(sum( imfilter( a.-b, Kernel.gaussian(5)).^2 )) +# sqrt(sum((imfilter(a, kernel).-imfilter(b, kernel)).^2)) + +# also do MMD version + + +# Next step is to define a function that applies a transform to the image. This +# transform consists of a translation and a rotation +function transformImage_SE2(img::AbstractMatrix, + trans::AbstractVector{<:Real}, + rot::Real, + gridscale::Real=1 ) + # + tf_ = Translation( gridscale.*trans... ) + img_r = imrotate(img, rot) + tf_img = warp(img_r, tf_, degree=ImageTransformations.Constant()) + + # replace NaN w/ 0 + mask = findall(x->isnan(x), tf_img) + tf_img[mask] .= 0.0 + return tf_img +end + + +# now we can combine the two into an evaluation function +function evaluateTransform( a::AbstractMatrix, + b::AbstractMatrix, + trans::AbstractVector{<:Real}, + rot::Real, + gridscale::Real=1 ) + # + # transform image + bp = transformImage_SE2(b, trans, rot, gridscale) + + # get matching padded views + ap, bpp = paddedviews(0.0, a, bp) + return getMismatch(ap,bpp) +end + + + +# \ No newline at end of file diff --git a/src/images/ScatterAlignPose2.jl b/src/images/ScatterAlignPose2.jl new file mode 100644 index 000000000..c266ed54e --- /dev/null +++ b/src/images/ScatterAlignPose2.jl @@ -0,0 +1,315 @@ + + +import IncrementalInference: getSample +import Base: convert, show +# import DistributedFactorGraphs: getManifold +import ApproxManifoldProducts: sample + +using .Images + +export ScatterAlignPose2, PackedScatterAlignPose2 + +export overlayScatter, overlayScatterMutate + +""" +$TYPEDEF + +This is but one incarnation for how radar alignment factor could work, treat it as a starting point. + +Notes +- Stanard `cvt` argument is lambda function to convert incoming images to user convention of image axes, + - **Geography map default** `cvt` flips image rows so that Pose2 +xy-axes corresponds to img[-x,+y] + - i.e. rows down is "North" and columns across from top left corner is "East". +- Use rescale to resize the incoming images for lower resolution (faster) correlations +- Both images passed to the construct must have the same type some matrix of type `T`. + +Example +------- +```julia +arp2 = ScatterAlignPose2(img1, img2, 2) # e.g. 2 meters/pixel +``` + +See also: [`overlayScanMatcher`](@ref) +""" +Base.@kwdef struct ScatterAlignPose2{ H1 <: Union{<:ManifoldKernelDensity, <:HeatmapGridDensity}, + H2 <: Union{<:ManifoldKernelDensity, <:HeatmapGridDensity} } <: IIF.AbstractManifoldMinimize + """ reference image for scan matching. """ + cloud1::H1 + """ test image to scan match against the reference image. """ + cloud2::H2 + """ Common grid scale for both images -- i.e. units/pixel. + Constructor uses two arguments `gridlength`*`rescale=1`=`gridscale`. + Arg 0 < `rescale` ≤ 1 is also used to rescale the images to lower resolution for speed. """ + gridscale::Float64 = 1.0 + """ how many heatmap sampled particles to use for mmd ailgnment """ + sample_count::Int = 100 + """ bandwidth to use for mmd """ + bw::Float64 = 1.0 +end + + +# replace inner constructor with transform on image +ScatterAlignPose2(im1::AbstractMatrix{T}, + im2::AbstractMatrix{T}, + domain::Tuple{<:AbstractVector{<:Real},<:AbstractVector{<:Real}}; + sample_count::Integer=75, + bw::Real=5e-5, # from a sensitivity analysis with marine radar data (50 or 100 samples) + rescale::Real=1, + N::Integer=1000, + cvt = (im)->reverse(Images.imresize(im,trunc.(Int, rescale.*size(im))),dims=1) + ) where {T} = ScatterAlignPose2(;cloud1=HeatmapGridDensity(cvt(im1),domain,N=N), + cloud2=HeatmapGridDensity(cvt(im2),domain,N=N), + gridscale=float(rescale), + sample_count, + bw ) +# + +getManifold(::IIF.InstanceType{<:ScatterAlignPose2}) = getManifold(Pose2Pose2) + +Base.@kwdef struct _FastRetract{M_ <: AbstractManifold, T} + M::M_ = SpecialEuclidean(2) + pTq::T = ProductRepr(MVector(0,0.0), MMatrix{2,2}(1.0, 0.0, 0.0, 1.0)) + p::ProductRepr{Tuple{SVector{2, Float64}, SMatrix{2, 2, Float64, 4}}} = ProductRepr(SA[0.0;0.0], SMatrix{2,2}(1.0, 0, 0, 1)) +end + +function (_user::_FastRetract)(pCq::AbstractVector{<:Real}) + retract!(_user.M, _user.pTq, _user.p, hat(_user.M, _user.p, pCq)) + + return _user.pTq +end + +function getSample( cf::CalcFactor{<:ScatterAlignPose2} ) + # + pVi, = sample(cf.factor.cloud1, cf.factor.sample_count) + pts2, = sample(cf.factor.cloud2, cf.factor.sample_count) + + M = getManifold(Pose2) + e0 = ProductRepr(SVector(0.0,0.0), SMatrix{2,2}(1.0, 0.0, 0.0, 1.0)) + + # precalc SE2 points + R0 = e0.parts[2] + # source memory + qVj_ = map(pt->ProductRepr(SVector(pt...), R0), pts2) + # destination memory + _pVj_! = map(pt->ProductRepr(MVector(pt...), MMatrix{2,2}(0.0,0.0,0.0,0.0)), pts2) + _pVj! = map(x->x.parts[1], _pVj_!) + + function pVj(xyr) + pTq! = _FastRetract() + for i in eachindex(qVj_) + Manifolds.compose!(M, _pVj_![i], pTq!(xyr), qVj_[i]) + # _pVj![i][:] = Manifolds.compose(M, pTq!(xyr), qVj_[i]).parts[1] + end + + _pVj! + end + + # # get the current relative transform estimate + # pTq! = _FastRetract() # not really any faster yet + # # not efficient, but okay for here + # # pTq(xyr) = exp(M, e0, hat(M, e0, xyr)) + # # move other points with relative transform + # pVj(xyr) = map(pt->Manifolds.compose(M, pTq!(xyr), pt).parts[1], qVj_) + + bw = SA[cf.factor.bw;] + cost(xyr) = mmd(M.manifold[1], pVi, pVj(xyr), length(pVi), length(qVj_), cf._allowThreads; bw) + + # return mmd as residual for minimization + res = Optim.optimize(cost, [10*randn(2); 0.1*randn()] ) + + M, e0, hat(M, e0, res.minimizer) +end + + +function (cf::CalcFactor{<:ScatterAlignPose2})(Xtup, wPp, wPq) + # + + # TODO move M to CalcFactor, follow IIF #1415 + M, e0, pXq = Xtup + + # get the current relative transform estimate + wPq_ = Manifolds.compose(M, wPp, exp(M, e0, pXq)) + + #TODO allocalte for vee! see Manifolds #412, fix for AD + Xc = zeros(3) + vee!(M, Xc, wPq, log(M, wPq, wPq_)) + return Xc +end + + + + +""" + $SIGNATURES + +Overlay the two images from `AlignRadarPose2` with the first (red) fixed and transform the second image (blue) according to `tf`. + +Notes: +- `tf` is a Manifolds.jl type `::ProductRepr` (or newer `::ArrayPartition`) to represent a `SpecialEuclidean(2)` manifold point. +- `user_offset` translates both are the same. + +See also: [`plotScatterAlign`](@ref) +""" +function overlayScatter(sap::ScatterAlignPose2, + trans::AbstractVector{<:Real}=[0;0.0], + rot::Real=0.0; + user_coords = [trans; rot], + score=Ref(0.0), + sample_count::Integer=sap.sample_count, + showscore::Bool=true, + findBest::Bool=true ) + # + + # # sample points from the scatter field + tfg = initfg() + addVariable!(tfg, :x0, Pose2) + addVariable!(tfg, :x1, Pose2) + addFactor!(tfg, [:x0;:x1], sap; graphinit=false) + meas = sampleFactor(tfg, :x0x1f1)[1] + + M, pts1, pts2_ = meas + pts2 = map(pt->pt.parts[1], pts2_) + e0 = identity_element(M) + # R0 = e0.parts[2] + + # not efficient, but okay for here + pTq(xyr=user_coords) = exp(M, e0, hat(M, e0, xyr)) + + + best_coords = zeros(3) + if findBest + # keep tfg separately so that optim can be more efficient + # tfg = initfg() + ev_(xyr) = calcFactorResidualTemporary( sap, (Pose2,Pose2), + meas, + (e0,pTq(xyr)); + tfg )[1] + # + score[] = ev_(user_coords) + best = Optim.optimize(ev_, user_coords, BFGS()) + best_coords .= round.(best.minimizer,digits=3) + + if showscore + @info "overlayScatter score" score[] string(best_coords) best.minimum # user_offset + end + end + + # transform points1 to frame of 2 -- take p as coordinate expansion point + pts2T_u = map(pt->Manifolds.compose(M, inv(M, pTq()), pt).parts[1], pts2_) + pts2T_b = map(pt->Manifolds.compose(M, inv(M, pTq(best_coords)), pt).parts[1], pts2_) + + @cast __pts1[i,j] := pts1[j][i] + @cast __pts2[i,j] := pts2[j][i] + @cast __pts2Tu[i,j] := pts2T_u[j][i] + @cast __pts2Tb[i,j] := pts2T_b[j][i] + + (;pP1=__pts1, qP2=__pts2, pP2_u=__pts2Tu, pP2_b=__pts2Tb, best_coords, user_coords, score=round(score[],digits=5)) #, user_offset) +end + + +function overlayScatterMutate(sap_::ScatterAlignPose2; + sample_count::Integer = sap_.sample_count, + bw::Real = sap_.bw, + kwargs... ) + # + sap = ScatterAlignPose2(sap_.cloud1, sap_.cloud2, sap_.gridscale, sample_count, float(bw)) + Caesar.overlayScatter(sap; kwargs...); +end + + +function Base.show(io::IO, sap::ScatterAlignPose2{H1,H2}) where {H1,H2} + printstyled(io, "ScatterAlignPose2{", bold=true, color=:blue) + println(io) + printstyled(io, " H1 = ", color=:magenta) + println(io, H1) + printstyled(io, " H2 = ", color=:magenta) + println(io, H2) + printstyled(io, " }", color=:blue, bold=true) + println(io) + if H1 <: HeatmapGridDensity + println(io, " size(.data): ", size(sap.cloud1.data)) + println(io, " min/max: ", round(minimum(sap.cloud1.data),digits=3), "/", round(maximum(sap.cloud1.data),digits=3)) + end + if H2 <: HeatmapGridDensity + println(io, " size(.data): ", size(sap.cloud2.data)) + println(io, " min/max: ", round(minimum(sap.cloud2.data),digits=3), "/", round(maximum(sap.cloud2.data),digits=3)) + end + + println(io, " gridscale: ", sap.gridscale) + println(io, " sample_count: ", sap.sample_count) + println(io, " bw: ", sap.bw) + nothing +end + +Base.show(io::IO, ::MIME"text/plain", sap::ScatterAlignPose2) = show(io, sap) +Base.show(io::IO, ::MIME"application/juno.inline", sap::ScatterAlignPose2) = show(io, sap) + + + +## ========================================================================================= +## Factor serialization below +## ========================================================================================= + +Base.@kwdef struct PackedScatterAlignPose2 <: PackedInferenceType + cloud1::String # PackedHeatmapGridDensity # change to String + cloud2::String # PackedHeatmapGridDensity # change to String + gridscale::Float64 = 1.0 + sample_count::Int = 50 + bw::Float64 = 0.01 +end + +function convert(::Type{<:PackedScatterAlignPose2}, arp::ScatterAlignPose2) + + function _toDensityJson(dens::ManifoldKernelDensity) + convert(PackedSamplableBelief,dens) + end + function _toDensityJson(dens::HeatmapGridDensity) + cloud1 = convert(PackedHeatmapGridDensity,dens) + JSON2.write(cloud1) + end + + cloud1_ = _toDensityJson(arp.cloud1) + cloud2_ = _toDensityJson(arp.cloud2) + + PackedScatterAlignPose2(; + cloud1 = cloud1_, + cloud2 = cloud2_, + gridscale = arp.gridscale, + sample_count = arp.sample_count, + bw = arp.bw ) +end + +function convert(::Type{<:ScatterAlignPose2}, parp::PackedScatterAlignPose2) + # first understand the schema friendly belief type to unpack + _cloud1 = JSON2.read(parp.cloud1) + _cloud2 = JSON2.read(parp.cloud2) + # @show _cloud1 + # @show parp.cloud1 + # _cloud2[Symbol("_type")] + PackedT1 = DFG.getTypeFromSerializationModule(_cloud1[Symbol("_type")]) + PackedT2 = DFG.getTypeFromSerializationModule(_cloud2[Symbol("_type")]) + # re-unpack into the local PackedT (marshalling) + # TODO check if there is perhaps optimization to marshal directly from _cloud instead - maybe `obj(;namedtuple...)` + # pcloud1 = JSON2.read(parp.cloud1, PackedT1) + # pcloud2 = JSON2.read(parp.cloud2, PackedT2) + + # outT + outT1 = convert(SamplableBelief, PackedT1) + outT2 = convert(SamplableBelief, PackedT2) + + # @info "deserialize ScatterAlignPose2" typeof(_cloud1) PackedT1 outT1 + + # convert from packed schema friendly to local performance type + cloud1 = convert(outT1, parp.cloud1) + cloud2 = convert(outT2, parp.cloud2) + + # and build the final object + ScatterAlignPose2( + cloud1, + cloud2, + parp.gridscale, + parp.sample_count, + parp.bw ) +end + +# \ No newline at end of file diff --git a/src/images/imagedata.jl b/src/images/imagedata.jl index 1755c2313..5160c42fb 100644 --- a/src/images/imagedata.jl +++ b/src/images/imagedata.jl @@ -1,8 +1,35 @@ -@info "Loading Caesar tools related to ImageMagick." +@info "Caesar.jl is loading tools using ImageMagick.jl." +export toFormat export fetchDataImage + +""" + $SIGNATURES + +Convert image to PNG bytestream. + +Notes +- `using ImageMagick, FileIO` + +DevNotes +- TODO somehow consolidate with `MIME"image/png"`. + +See also: [`makeImage`](@ref) +""" +function toFormat(format::DataType, + img::AbstractMatrix{<:Colorant} ) + # + io = IOBuffer() + pngSm = Stream(format, io) + save(pngSm, img) # think FileIO is required for this + take!(io) +end + +toFormat(img::AbstractMatrix{<:Colorant}) = toFormat(format"PNG", img) + + """ $SIGNATURES diff --git a/src/images/imagedraw.jl b/src/images/imagedraw.jl new file mode 100644 index 000000000..d23bf6d02 --- /dev/null +++ b/src/images/imagedraw.jl @@ -0,0 +1,51 @@ + +@info "Caesar.jl is loading tools using ImageDraw.jl." + +using .ImageDraw +using .Colors + +export makeImage! + + +function makeImage!(pc::Caesar._PCL.PointCloud, + x_domain::Tuple{<:Real,<:Real}=(-1000,1000), + y_domain::Tuple{<:Real,<:Real}=x_domain; + pose=nothing, + ppose=nothing, + rows::Integer=1000, + cols::Integer=rows, + color::C=Gray(0.1), + trajCol=Gray(1.0), + img::AbstractMatrix{<:Colorant} = Gray.(zeros(rows,cols)), + circle_size::Real=1, + drawkws... ) where {C <: Colorant} + # + + x_range = (x_domain[2]-x_domain[1]) + y_range = (y_domain[2]-y_domain[1]) + gridsize_x = x_range/rows + gridsize_y = y_range/cols + + for pt in pc.points + _x = trunc(Int, ( pt.x-x_domain[1])/gridsize_x ) # + rows/2 + _y = trunc(Int, (-pt.y-y_domain[1])/gridsize_y ) # + cols/2 + draw!( img, Ellipse(CirclePointRadius(_x, _y, circle_size)), color; drawkws... ) #; thickness = 1, fill = true)) ) + end + + # draw pose location + if !(pose isa Nothing) + _x = trunc(Int, ( pose[1]-x_domain[1])/gridsize_x ) # + rows/2 + _y = trunc(Int, (-pose[2]-y_domain[1])/gridsize_y ) # + cols/2 + + draw!( img, Ellipse(CirclePointRadius(_x, _y, 4)), trajCol ) + + if !(ppose isa Nothing) + __x = trunc(Int, ( pose[1]-x_domain[1])/gridsize_x ) # + rows/2 + __y = trunc(Int, (-pose[2]-y_domain[1])/gridsize_y ) # + cols/2 + + draw!( img, LineSegment(Point(_x, _y), Point(__x, __y)), trajCol ) + end + end + + return img +end diff --git a/src/images/images.jl b/src/images/images.jl index aca7cb979..693fca464 100644 --- a/src/images/images.jl +++ b/src/images/images.jl @@ -8,6 +8,7 @@ export writevideo export imhcatPretty, csmAnimationJoinImgs, csmAnimateSideBySide export makeVideoFromData + """ $SIGNATURES @@ -48,12 +49,10 @@ end function writevideo(fname::AbstractString, imgs::AbstractVector{<:AbstractArray{<:Colorant,2}}; - overwrite=true, fps::Int=30, options=``, - player::AbstractString="", - pix_fmt="yuv420p" ) + kwargs... ) # @cast imgstack[r,c,k] := imgs[k][r,c] - writevideo(fname, imgstack, overwrite=overwrite, fps=fps, options=options, player=player, pix_fmt=pix_fmt) + writevideo(fname, imgstack; kwargs...) end diff --git a/src/plotting/ScatterAlignPlotting.jl b/src/plotting/ScatterAlignPlotting.jl new file mode 100644 index 000000000..b25f89430 --- /dev/null +++ b/src/plotting/ScatterAlignPlotting.jl @@ -0,0 +1,41 @@ + +@info "Caesar.jl is including ScatterAlign tools associated with Gadfly.jl." + +using .Gadfly + +export plotScatterAlign + + +""" + $SIGNATURES + +Plot name tuple output from [`overlayScatter`](@ref) + +See also: [`overlayScatterMutate`](@ref) +""" +function plotScatterAlign(snt::NamedTuple; title::String="") + N = size(snt.pP1,2) + + pP1 = Gadfly.layer(x=snt.pP1[1,:],y=snt.pP1[2,:],color=[1;zeros(N-1)]) + qP2 = Gadfly.layer(x=snt.qP2[1,:],y=snt.qP2[2,:],color=[0;ones(N-2);2]) + pP2_u = Gadfly.layer(x=snt.pP2_u[1,:],y=snt.pP2_u[2,:],color=[0;2*ones(N-1)]) + pP2_b = Gadfly.layer(x=snt.pP2_b[1,:],y=snt.pP2_b[2,:],color=[0;2*ones(N-1)]) + + uo = haskey(snt, :user_offset) ? "$(round.(snt.user_offset, digits=3))" : "" + + ar = Gadfly.Coord.cartesian(; aspect_ratio=1) + H1 = Gadfly.plot(pP1, qP2, ar, Guide.title("body frame data."*title)) + H2 = Gadfly.plot(pP1, pP2_u, ar, Guide.title("User transform: $(round.(snt.user_coords,digits=3))")) + H3 = Gadfly.plot(pP1, pP2_b, ar, Guide.title("Best fit: $(snt.best_coords)\nscore=$(snt.score)")) + + hstack(H1,H2, H3) +end + +plotScatterAlign( sap::ScatterAlignPose2; + sample_count::Integer = sap.sample_count, + bw::Real = sap.bw, + kw... ) = plotScatterAlign(overlayScatterMutate(sap; sample_count, bw); kw...) +# + + +# \ No newline at end of file diff --git a/src/ros/Utils/RosbagSubscriber.jl b/src/ros/Utils/RosbagSubscriber.jl index fe7bb17f5..e3f113d1f 100644 --- a/src/ros/Utils/RosbagSubscriber.jl +++ b/src/ros/Utils/RosbagSubscriber.jl @@ -66,6 +66,7 @@ function loop!(rbs::RosbagSubscriber, args...) return if isa(msgT, PyObject) # update the syncBuffer rbs.syncBuffer[rbs.nextMsgChl] = rbs.nextMsgTimestamp = getROSPyMsgTimestamp(msgT) + @debug "RosbagSubscriber got msg $(rbs.nextMsgChl)" # call the callback rbs.callbacks[rbs.nextMsgChl](msg, args...) true @@ -76,14 +77,16 @@ function loop!(rbs::RosbagSubscriber, args...) end end -function (rbs::RosbagSubscriber)(chl::AbstractString, - callback::Function, - args...; - msgType=nothing) +function (rbs::RosbagSubscriber)( chl::AbstractString, + ::Type{MT}, + callback::Function, + args::Tuple; + msgType=nothing ) where {MT <: RobotOS.AbstractMsg} # cn = Symbol(string(chl)) push!(rbs.channels, cn) - rbs.callbacks[cn] = (m)->callback(m,args...) + # include the type converter, see ref: https://github.com/jdlangs/RobotOS.jl/blob/21a7088461a21bc9b24cd2763254d5043d5b1800/src/callbacks.jl#L23 + rbs.callbacks[cn] = (m)->callback(convert(MT,m[2]),args...) rbs.syncBuffer[cn] = (unix2datetime(0), 0) rbs.readers[cn] = RosbagParser(rbs.bagfile, chl) end diff --git a/src/ros/Utils/rosbagReader.py b/src/ros/Utils/rosbagReader.py index f5a8229de..05f3ceab5 100644 --- a/src/ros/Utils/rosbagReader.py +++ b/src/ros/Utils/rosbagReader.py @@ -18,7 +18,9 @@ def __init__(self, bag_file_name, topic_name): def get_next_message(self): if (self.idx < self.topic_size): - topic, msg, time = self.bag_contents.next() + topic, msg, time = next(self.bag_contents) + # https://stackoverflow.com/questions/18547878/attribute-error-next + # topic, msg, time = self.bag_contents.__next__() self.idx += 1 return topic, msg, time else: diff --git a/src/zmq/ZmqCaesar.jl b/src/zmq/ZmqCaesar.jl index 479f28cd7..2e5fec97a 100644 --- a/src/zmq/ZmqCaesar.jl +++ b/src/zmq/ZmqCaesar.jl @@ -1,6 +1,6 @@ module ZmqCaesar using Requires - using ZMQ + using ..ZMQ using JSON using Distributions, IncrementalInference, DistributedFactorGraphs, Caesar using Unmarshal diff --git a/src/zmq/services/ZmqServer.jl b/src/zmq/services/ZmqServer.jl index 43c5d6a22..e215b5233 100644 --- a/src/zmq/services/ZmqServer.jl +++ b/src/zmq/services/ZmqServer.jl @@ -1,5 +1,5 @@ -using ZMQ -using Caesar, Caesar.ZmqCaesar +# using ..ZMQ +# using Caesar, Caesar.ZmqCaesar export start diff --git a/src/zmq/services/session.jl b/src/zmq/services/session.jl index c15de0503..8a660e89a 100644 --- a/src/zmq/services/session.jl +++ b/src/zmq/services/session.jl @@ -49,8 +49,8 @@ function addFactor(configDict, fg, requestDict)::Dict{String, Any} # Right, carrying on... factor = nothing try - # FIXME, use appropriate new DFG method rather than _evalType - factType = _evalType(factorRequest["factorType"]) + factType = DFG.getTypeFromSerializationModule(factorRequest["factorType"]) + # factType = _evalType(factorRequest["factorType"]) # FIXME, use appropriate new DFG method rather than _evalType factor = convert(factType, packedFactor) catch ex io = IOBuffer() diff --git a/test/pcl/testPointCloud2.jl b/test/pcl/testPointCloud2.jl new file mode 100644 index 000000000..a835b23a4 --- /dev/null +++ b/test/pcl/testPointCloud2.jl @@ -0,0 +1,95 @@ +# requires python3 and rospy to be installed + + +# ENV["PYTHON"] = "/usr/bin/python3" +# Pkg.build("PyCall") +# using PyCall +# using RobotOS + +using Colors +using Caesar +using BSON + +using Test +using Pkg + +# import Caesar._PCL: FieldMapper, createMapping, PointCloud, PointField, PCLPointCloud2, Header, asType, _PCL_POINTFIELD_FORMAT, FieldMapping, MsgFieldMap, FieldMatches + + +## +@testset "test Caesar._PCL.PCLPointCloud2 to Caesar._PCL.PointCloud converter." begin +## + +# testdatafile = joinpath( dirname(dirname(Pkg.pathof(Caesar))), "test", "testdata", "_PCLPointCloud2.bson") +# load presaved test data to test the coverter +# BSON.@load testdatafile PointCloudRef PointCloudTest + +## build PCLPointCloud2 to be converted + +datafile = joinpath( dirname(dirname(Pkg.pathof(Caesar))), "test", "testdata", "_PCLPointCloud2_15776.dat") +fid = open(datafile,"r") +data = read(fid) +close(fid) + +## + +pc2 = Caesar._PCL.PCLPointCloud2(; + header = Caesar._PCL.Header(; + seq = 92147, + stamp = 0x0005b24647c4af10, + frame_id = "velodyne" + ), + height = 1, + width = 493, + fields = [ + Caesar._PCL.PointField("x", 0x00000000, Caesar._PCL._PCL_FLOAT32, 0x00000001), + Caesar._PCL.PointField("y", 0x00000004, Caesar._PCL._PCL_FLOAT32, 0x00000001), + Caesar._PCL.PointField("z", 0x00000008, Caesar._PCL._PCL_FLOAT32, 0x00000001), + Caesar._PCL.PointField("intensity", 0x00000010, Caesar._PCL._PCL_FLOAT32, 0x00000001) + ], + point_step = 32, + row_step = 15776, + is_dense = 1, + data +) + + +## + +show(pc2) + +# test major unpacking / type conversion needed by ROS.sensor_msgs.pointcloud2 +pc = Caesar._PCL.PointCloud(pc2) + +show(pc) + +@test pc.height == 1 +@test pc.width == 493 +@test pc.is_dense + +@test length(pc.points) == 493 + +## + +# truncated copy of reference data on good to 1e-3 +@test isapprox( [-0,0,0], pc.points[1].data[1:3], atol=5e-3) +@test isapprox( [-1.56486,1.09851,0], pc.points[2].data[1:3], atol=5e-3) +@test isapprox( [-793.383,556.945,0], pc.points[3].data[1:3], atol=5e-3) +@test isapprox( [-0,0,0], pc.points[4].data[1:3], atol=5e-3) +@test isapprox( [-1.56148,1.10331,0], pc.points[5].data[1:3], atol=5e-3) +@test isapprox( [-788.548,557.169,0], pc.points[6].data[1:3], atol=5e-3) +@test isapprox( [-790.109,558.273,0], pc.points[7].data[1:3], atol=5e-3) +@test isapprox( [-791.671,559.376,0], pc.points[8].data[1:3], atol=5e-3) +@test isapprox( [-793.232,560.479,0], pc.points[9].data[1:3], atol=5e-3) +@test isapprox( [-794.794,561.583,0], pc.points[10].data[1:3], atol=5e-3) + + +# for (i,pt) in enumerate(pc.points) +# @test isapprox( PointCloudRef[1][i,1:3], pt.data[1:3]; atol=1e-6) +# end + + +## +end + +# \ No newline at end of file diff --git a/test/runtests.jl b/test/runtests.jl index e64958c43..f052cfa89 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -1,11 +1,17 @@ # using RoMEPlotting -using ZMQ -using Caesar, Caesar.ZmqCaesar -using IncrementalInference, RoME +# using IncrementalInference, RoME using Test ## See tests in DFG, IIF, and RoME for more complete coverage of features +include("pcl/testPointCloud2.jl") + +include("testPose2AprilTag4Corner.jl") + + +using ZMQ +using Caesar, Caesar.ZmqCaesar + @testset "ZMQ Interface" begin # Unit tests include("multilangzmq/callbackCompatibilityTest.jl") @@ -16,6 +22,5 @@ using Test # include("multilangzmq/serverTest.jl") end -include("testPose2AprilTag4Corner.jl") - +# include("testScatterAlignPose2.jl") diff --git a/test/testScatterAlignPose2.jl b/test/testScatterAlignPose2.jl new file mode 100644 index 000000000..ff0f194af --- /dev/null +++ b/test/testScatterAlignPose2.jl @@ -0,0 +1,249 @@ +# test ScatterAlignPose2 + +using Test +using Images +using Caesar +using Distributions +using Manifolds + +# test plotting helper functions +using Gadfly +using Random + +import Rotations as _Rot + +## +@testset "Test ScatterAlignPose2" begin +## + +x = -10:0.1:10; +y = -10:0.1:10; + +x = -10:0.1:10; + +σ = 0.1 + +g = (x,y)->pdf(MvNormal([3.;0],[σ;σ]),[x;y]) + pdf(MvNormal([8.;0.0],[σ;σ]),[x;y]) + pdf(MvNormal([0;5.0],[σ;σ]),[x;y]) + +bIM1 = zeros(length(x),length(y)) +bIM2 = zeros(length(x),length(y)) + +oT = [2.; 0] +oΨ = pi/6 + +M = SpecialEuclidean(2) +e0 = identity_element(M) +pCq = [oT;oΨ] +qGp = inv(M, exp(M, e0, hat(M, e0, pCq))) +qTp = affine_matrix(M, qGp ) + +qCp = vee(M, e0, log(M, e0, qGp)) + +## + +for (i,x_) in enumerate(x), (j,y_) in enumerate(y) + bIM1[i,j] = g(x_,y_) + v = qTp*[x_;y_;1.0] + _x_, _y_ = v[1], v[2] + bIM2[i,j] = g(_x_, _y_) +end + +sap = ScatterAlignPose2(bIM1, bIM2, (x,y); sample_count=100, bw=1.0, cvt=(im)->im) + +# requires IIF at least v0.25.6 +@test sample(sap.cloud1,1) isa Tuple +@test sample(sap.cloud2,10)[1] isa AbstractArray + +## test plotting function + +snt = overlayScatterMutate(sap; sample_count=50, bw=1., user_coords=[0.;0;0*pi/6]); # , user_offset=[0.;0;0.]); +plotScatterAlign(snt; title="\npCq=$(round.(pCq,digits=2))") + +## + +# inverse for q --> p +@test isapprox( pCq[1:2], snt.best_coords[1:2]; atol=0.3 ) +@test isapprox( pCq[3], snt.best_coords[3]; atol=0.2 ) + + + +## check packing and unpacking + +psap = convert(PackedScatterAlignPose2, sap); +sap_ = convert(ScatterAlignPose2, psap); + +@test sap.gridscale == sap_.gridscale +@test sap.sample_count == sap_.sample_count +@test sap.bw == sap_.bw + +@test isapprox(sap.cloud1, sap_.cloud1, mmd_tol=1e-2) +@test isapprox(sap.cloud2, sap_.cloud2, mmd_tol=1e-2) + +## check that optimize works (using the same tfg) + +tfg = initfg() +getSolverParams(tfg).attemptGradients = false +M = getManifold(sap) +e0 = identity_element(M) +meas = sample(sap.cloud1,100)[1], [ProductRepr(sample(sap.cloud2,1)[1][1],[1 0; 0 1.]) for _ in 1:100], M +δ(x) = calcFactorResidualTemporary(sap, (Pose2,Pose2), meas, (e0,ProductRepr(x[1:2],_Rot.RotMatrix(x[3]))) , tfg=tfg)[1] + +@show δ([0;0;0.]); +@show δ([1.;0;0.]); + +@test isapprox(δ([0;0;0.]), δ([0;0;0.]); atol=1e-6) +@test isapprox(δ([10;0;0.]), δ([10;0;0.]); atol=1e-6) +@test !isapprox( δ([0;0;0.]), δ([0.1;0;0.]), atol=1e-6 ) +@test !isapprox( δ([0;0;0.]), δ([0;0;0.1]), atol=1e-6 ) + + +## build into graph + +fg = initfg() +getSolverParams(fg).inflateCycles=1 + +addVariable!(fg, :x0, Pose2) +addVariable!(fg, :x1, Pose2) + +addFactor!(fg, [:x0;], PriorPose2(MvNormal([0;0;0.],[0.01;0.01;0.01]))) +addFactor!(fg, [:x0;:x1], sap, inflation=0.0) + +## use in graph + +X1 = approxConvBelief(fg, :x0x1f1, :x1) + +c1 = AMP.makeCoordsFromPoint(getManifold(Pose2), mean(X1)) +@show c1 + +## + +@test isapprox( pCq[1:2], c1[1:2], atol=0.5 ) +@test isapprox( pCq[3], c1[3], atol=0.3 ) + +## check save and load of sap + +saveDFG("/tmp/caesar/test_sap", fg) +fg_ = loadDFG("/tmp/caesar/test_sap") + +## + +sap = getFactorType(fg, :x0x1f1) +sap_ = getFactorType(fg_, :x0x1f1) + +@test isapprox( sap.cloud1, sap_.cloud1) +@test isapprox( sap.cloud2, sap_.cloud2) + +@test sap.gridscale == sap_.gridscale +@test sap.sample_count == sap_.sample_count +@test sap.bw == sap_.bw + +## +end + + + + + + + + + + +@testset "test ScatterAlignPose2 with MKD direct" begin +## + +# setup + +oT = [2.; 0] +oΨ = pi/6 + +M = SpecialEuclidean(2) +e0 = identity_element(M) +pCq = [oT;oΨ] +qTp = affine_matrix(M, exp(M, e0, hat(M, e0, pCq))) + +## + +# Points in XY only + +# g = (x,y)->pdf(MvNormal([3.;0],[σ;σ]),[x;y]) + pdf(MvNormal([8.;0.0],[σ;σ]),[x;y]) + pdf(MvNormal([0;5.0],[σ;σ]),[x;y]) +p1 = vcat([0.1*randn(2).+[3;0.] for i in 1:50], [0.1*randn(2)+[8.;0] for i in 1:50], [0.1*randn(2)+[0;5.] for i in 1:50]) +# foreach(pt->(pt[1] += 100), p1) +shuffle!(p1) +P1 = manikde!(getManifold(Point2), p1) + +p2 = vcat([0.1*randn(2).+[3;0.] for i in 1:50], [0.1*randn(2)+[8.;0] for i in 1:50], [0.1*randn(2)+[0;5.] for i in 1:50]) +# foreach(pt->(pt[1] += 100), p2) +# adjust points +for (i,pt) in enumerate(p2) + v = qTp*[pt;1.0] + pt[1:2] .= v[1:2] +end +shuffle!(p2) +P2 = manikde!(getManifold(Point2), p2) + +sap = ScatterAlignPose2(;cloud1=P1, cloud2=P2, sample_count=100, bw=1.0) + +## test plotting function + +snt = overlayScatterMutate(sap; sample_count=100, bw=2.0, user_coords=[0.;0;0*pi/6]); +plotScatterAlign(snt; title="\npCq=$(round.(pCq,digits=2))") + +## + +# inverse for q --> p +@test isapprox( oT, snt.best_coords[1:2]; atol=1.0 ) +@test isapprox( oΨ, snt.best_coords[3]; atol=0.5 ) + +## + +fg = initfg() +getSolverParams(fg).inflateCycles=1 + +addVariable!(fg, :x0, Pose2) +addVariable!(fg, :x1, Pose2) + +addFactor!(fg, [:x0], PriorPose2(MvNormal([0.01;0.01;0.01]))) +addFactor!(fg, [:x0;:x1], sap, inflation=0.0) + +## check residual calculation + +# see #1415 +meas = sample(P1,100)[1], [ProductRepr([0;0.],[1 0; 0 1.]) for _ in 1:100], M +δ1 = calcFactorResidualTemporary(sap, (Pose2,Pose2), meas, (e0,e0)) + +meas = sample(P1,100)[1], [ProductRepr(sample(P2,1)[1][1],[1 0; 0 1.]) for _ in 1:100], M +δ2 = calcFactorResidualTemporary(sap, (Pose2,Pose2), meas, (e0,e0)) + +# check different cloud samplings produce different residual values +@test !isapprox(δ1, δ2, atol=1e-4) + +## check that optimize works (using the same tfg) + +tfg = initfg() +# meas = sample(P1,100)[1], [ProductRepr(sample(P2,1)[1][1],[1 0; 0 1.]) for _ in 1:100], M +meas = sampleFactor(fg, :x0x1f1)[1] +δ(x) = calcFactorResidualTemporary(sap, (Pose2,Pose2), meas, (e0,ProductRepr(x[1:2],_Rot.RotMatrix(x[3]))), tfg=tfg)[1] + +@show δ([0;0;0.]) +@show δ([1.;0;0.]) + +@test !isapprox( δ([0;0;0.]), δ([1.;0;0.]), atol=1e-6 ) + + +## + +X1 = approxConvBelief(fg, :x0x1f1, :x1) +c1 = AMP.makeCoordsFromPoint(getManifold(Pose2), mean(X1)) +@show c1 + +## + +@test isapprox( pCq[1:2], c1[1:2], atol=0.5 ) +@test isapprox( pCq[3], c1[3], atol=0.3 ) + + +## +end + +# \ No newline at end of file diff --git a/test/testdata/_PCLPointCloud2_15776.dat b/test/testdata/_PCLPointCloud2_15776.dat new file mode 100644 index 000000000..de4435559 Binary files /dev/null and b/test/testdata/_PCLPointCloud2_15776.dat differ diff --git a/test/testdata/pcl_pc2_15776.cpp b/test/testdata/pcl_pc2_15776.cpp new file mode 100644 index 000000000..6cd06a747 --- /dev/null +++ b/test/testdata/pcl_pc2_15776.cpp @@ -0,0 +1,192 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2009-2011, Willow Garage, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + * $Id: example_OrganizedPointCloud.cpp 4258 2012-02-05 15:06:20Z daviddoria $ + * + */ + +// STL +#include +#include +#include + +// PCL +#include +#include + +#include + + +char* readdata (int* length) { + + std::ifstream is ("_PCLPointCloud2_15776.dat", std::ifstream::binary); + // int length = 0 + if (is) { + // get length of file: + is.seekg (0, is.end); + *length = is.tellg(); + is.seekg (0, is.beg); + + char * buffer = new char [*length]; + + std::cout << "Reading " << *length << " characters... "; + // read data as a block: + is.read (buffer,*length); + + if (is) + std::cout << "all characters read successfully." << std::endl; + else + std::cout << "error: only " << is.gcount() << " could be read" << std::endl; + is.close(); + + // ...buffer contains the entire file... + + // delete[] buffer; + return buffer; + } + char* dummy; + return dummy; +} + +int +main () +{ + // load binary data + int length = 0; + + // load test data from file, and check its length + char* buffer = readdata(&length); + std::cout << std::endl << "length is " << length << std::endl; + assert(length == 15776); + + // hand generate PCLPointCloud2 + pcl::PCLPointCloud2 pc2; + + // load data first + for (int i = 0; i < length; i++) { + pc2.data.push_back(buffer[i]); + } + + unsigned char* buffer_ = reinterpret_cast(buffer); + // sizeof arr + for (int i = 0; i < 5; i++) { + printf(" %02x", buffer_[i]); + printf("&%02x", pc2.data[i]); + } + std::cout << " ... "; + for (int i = length-6; i < length ; i++) { + printf(" %02x", buffer_[i]); + printf("&%02x", pc2.data[i]); + } + + std::cout << std::endl << "pc2.data size is " << pc2.data.size() << std::endl; + + + pcl::PCLHeader header; + header.seq = 92147; + header.frame_id = "velodyne"; + header.stamp = 1603389805080336; + + pcl::PCLPointField pf_x; + pf_x.name = "x"; + pf_x.offset = 0x00000000; + pf_x.datatype = pcl::PCLPointField::FLOAT32; + pf_x.count = 1; + pc2.fields.push_back(pf_x); + + pcl::PCLPointField pf_y; + pf_y.name = "y"; + pf_y.offset = 0x00000004; + pf_y.datatype = pcl::PCLPointField::FLOAT32; + pf_y.count = 1; + pc2.fields.push_back(pf_y); + + pcl::PCLPointField pf_z; + pf_z.name = "z"; + pf_z.offset = 0x00000008; + pf_z.datatype = pcl::PCLPointField::FLOAT32; + pf_z.count = 1; + pc2.fields.push_back(pf_z); + + pcl::PCLPointField pf_i; + pf_i.name = "intensity"; + pf_i.offset = 0x00000010; + pf_i.datatype = pcl::PCLPointField::FLOAT32; + pf_i.count = 1; + pc2.fields.push_back(pf_i); + + pc2.header = header; + pc2.height = 1; + pc2.width = 493; + pc2.point_step = 32; + pc2.row_step = 15776; + pc2.is_bigendian = false; + pc2.is_dense = 1; + + std::cout << "pc2 with header" << std::endl; + + // ============================================================================= + // Setup the cloud + using PointType = pcl::PointXYZ; + using CloudType = pcl::PointCloud; + CloudType::Ptr cloud (new CloudType); + + // do the conversion + pcl::fromPCLPointCloud2(pc2, *cloud); + + std::cout << "sizeof(PointT) = " << sizeof(PointType) << std::endl; + + // // Make the cloud a 10x10 grid + // cloud->height = 10; + // cloud->width = 10; + // cloud->is_dense = true; + // cloud->resize(cloud->height * cloud->width); + + // Output the (0,0) point + std::cout << (*cloud)(0,0) << std::endl; + std::cout << cloud->isOrganized() << std::endl; + for (int i=0; i< 10; i++) + std::cout << cloud->points[i] << std::endl; + // // Set the (0,0) point + // PointType p; p.x = 1; p.y = 2; p.z = 3; + // (*cloud)(0,0) = p; + + // // Confirm that the point was set + // std::cout << (*cloud)(0,0) << std::endl; + + delete[] buffer; + + return (0); +}