diff --git a/.github/workflows/BuildDocs.yml b/.github/workflows/BuildDocs.yml new file mode 100644 index 000000000..d4a5e661f --- /dev/null +++ b/.github/workflows/BuildDocs.yml @@ -0,0 +1,45 @@ +name: CI/CD Docs +on: + pull_request: + push: + branches: + - master + workflow_dispatch: + +jobs: + 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.10' + arch: x64 + - name: 'Pkgs for 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 }}" + export JULIA_PKG_PRECOMPILE_AUTO=0 + 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"))' + - name: 'Docs make.jl' + run: | + export JULIA_PKG_PRECOMPILE_AUTO=0 + export DOCUMENTER_DEBUG="true" + julia --project=${{ matrix.jlenv }} --color=yes ${{ matrix.makejl }} + env: + GITHUB_TOKEN: ${{ secrets.GITHUB_TOKEN }} + DOCUMENTER_KEY: ${{ secrets.DOCUMENTER_KEY }} + JULIA_PKG_SERVER: "" \ No newline at end of file diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 8137e7059..afe931eea 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -16,7 +16,7 @@ jobs: fail-fast: false matrix: version: - - '1.9' + - '1.10' - 'nightly' os: - ubuntu-latest @@ -62,7 +62,7 @@ jobs: fail-fast: false matrix: version: - - '1.9' + - '1.10' os: - ubuntu-latest arch: @@ -109,7 +109,7 @@ jobs: - uses: actions/checkout@v2 - uses: julia-actions/setup-julia@v1 with: - version: ~1.10.0-0 + version: '1.10' arch: x64 - uses: actions/cache@v1 env: @@ -126,40 +126,13 @@ jobs: git config --global user.email te@st.er - name: Run tests on Upstream Dev run: | + export JULIA_PKG_PRECOMPILE_AUTO=0 julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="RoME",rev="master"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="IncrementalInference",rev="master"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="ApproxManifoldProducts",rev="master"));' julia --project=@. --check-bounds=yes -e 'using Pkg; Pkg.add(PackageSpec(name="DistributedFactorGraphs",rev="master"));' + unset JULIA_PKG_PRECOMPILE_AUTO 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.9 - - 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/.gitignore b/.gitignore index 5c3f2075b..cd7993fee 100644 --- a/.gitignore +++ b/.gitignore @@ -5,5 +5,6 @@ examples/lcmserver/lcmtypes/rome/ examples/lcmserver/rome/ examples/tracking/simpleradar/exports/* docs/build +docs/Manifest.toml results/* build diff --git a/Project.toml b/Project.toml index e3d749a84..408fa7d9a 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.16.1" +version = "0.16.2" [deps] ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89" @@ -93,7 +93,7 @@ ImageCore = "0.8, 0.9, 0.10" ImageDraw = "0.2" ImageMagick = "1" IncrementalInference = "0.34, 0.35" -Interpolations = "0.14" +Interpolations = "0.14, 0.15" JLD2 = "0.3, 0.4" JSON = "0.20, 0.21" JSON2 = "0.3, 0.4" @@ -111,6 +111,7 @@ Reexport = "1" RoME = "0.23, 0.24" Rotations = "1.1" StaticArrays = "1" +Statistics = "1" StatsBase = "0.33, 0.34" TensorCast = "0.4" TimeZones = "1.3.1, 1.4" diff --git a/docs/Project.toml b/docs/Project.toml index a914d7185..19c147826 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -22,7 +22,6 @@ JLD2 = "033835bb-8acc-5ee8-8aae-3f567f8a3819" JSON = "682c06a0-de6a-54ab-a142-c8b1cf79cde6" KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d" KernelDensityEstimatePlotting = "c43967c8-f634-5d24-8eab-2867546b366b" -LCMCore = "0ea44823-1ff1-5b9a-8293-5fd55a38e746" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56" ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca" @@ -54,19 +53,18 @@ FunctionalStateMachine = "≥ 0.1" ImageCore = "≥ 0.7" ImageMagick = "≥ 0.7" Images = "≥ 0.24" -IncrementalInference = "≥ 0.13" +IncrementalInference = "≥ 0.35" JLD2 = "≥ 0.1" JSON = "≥ 0.18" KernelDensityEstimate = "≥ 0.5" KernelDensityEstimatePlotting = "≥ 0.1.4" -LCMCore = "≥ 0.5" NLsolve = "≥ 3" ProgressMeter = "≥ 0.9" Reexport = "≥ 0.2" -RoME = "≥ 0.7" +RoME = "≥ 0.23" Rotations = "≥ 0.13" TransformUtils = "≥ 0.2.2" Unmarshal = "≥ 0.3" YAML = "≥ 0.3" ZMQ = "≥ 1.0" -julia = "1.4" +julia = "1.10" diff --git a/docs/make.jl b/docs/make.jl index f48e24a87..7b4955495 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -3,10 +3,14 @@ using RoME using Colors using Images +import Caesar._PCL as _PCL + import IncrementalInference: fmcmc!, localProduct, prodmultiplefullpartials, prodmultipleonefullpartials, setfreeze! import IncrementalInference: cliqGibbs, packFromLocalPotentials!, treeProductDwn, updateFGBT!, upGibbsCliqueDensity import IncrementalInference: initfg, downGibbsCliqueDensity import IncrementalInference: solveGraphParametric, solveGraphParametric! +import IncrementalInference: _solveCCWNumeric! +import IncrementalInference: initParametricFrom! using KernelDensityEstimatePlotting # import KernelDensityEstimatePlotting: plotKDE @@ -16,6 +20,7 @@ using RoMEPlotting using DistributedFactorGraphs import DistributedFactorGraphs: showFactor, showVariable import DistributedFactorGraphs: deleteVariable! +import DistributedFactorGraphs: loadDFG, loadDFG! makedocs( modules = [Caesar, RoME, IncrementalInference, RoMEPlotting, KernelDensityEstimatePlotting, DistributedFactorGraphs], @@ -96,10 +101,12 @@ makedocs( "Literature" => [ "References" => "refs/literature.md" ], - ] + ], + # FIXME remove warnonly option once :missing_docs and :cross_references fixes are done + warnonly = Documenter.except(:autodocs_block, :docs_block, :doctest, :linkcheck, :eval_block, :example_block, :footnote, :linkcheck_remotes, :meta_block, :parse_error, :setup_block), # , :cross_references, :missing_docs # html_prettyurls = !("local" in ARGS), - ) - +) +# The possible Symbol values that can be passed to the function are: :autodocs_block, :cross_references, :docs_block, :doctest, :eval_block, :example_block, :footnote, :linkcheck_remotes, :linkcheck, :meta_block, :missing_docs, :parse_error, and :setup_block. deploydocs( repo = "github.com/JuliaRobotics/Caesar.jl.git", diff --git a/docs/src/concepts/2d_plotting.md b/docs/src/concepts/2d_plotting.md index a0a44979b..4843deff4 100644 --- a/docs/src/concepts/2d_plotting.md +++ b/docs/src/concepts/2d_plotting.md @@ -166,7 +166,7 @@ plotPose ### Debug With Local Graph Product Plot -One useful function is to check that data in the factor graph makes sense. While the full inference algorithm uses a Bayes (Junction) tree to assemble marginal belief estimates in an efficient manner, it is often useful for a straight forward graph based sanity check. The [`plotLocalProduct`](@ref) projects through [`approxConv`](@ref) each of the factors connected to the target variable and plots the result. This example looks at the loop-closure point around `:x0`, which is also pinned down by the only prior in the canonical Hexagonal factor graph. +One useful function is to check that data in the factor graph makes sense. While the full inference algorithm uses a Bayes (Junction) tree to assemble marginal belief estimates in an efficient manner, it is often useful for a straight forward graph based sanity check. The [`plotLocalProduct`](@ref) projects through [`approxConvBelief`](@ref) each of the factors connected to the target variable and plots the result. This example looks at the loop-closure point around `:x0`, which is also pinned down by the only prior in the canonical Hexagonal factor graph. ```julia @show ls(fg, :x0); # ls(fg, :x0) = [:x0f1, :x0x1f1, :x0l1f1] diff --git a/docs/src/concepts/arena_visualizations.md b/docs/src/concepts/arena_visualizations.md index 8407d4bcd..acf8ccbef 100644 --- a/docs/src/concepts/arena_visualizations.md +++ b/docs/src/concepts/arena_visualizations.md @@ -1,4 +1,4 @@ -# [Visualization 3D](@ref visualization_3d) +# [Visualization 3D](@id visualization_3d) ## Introduction diff --git a/docs/src/concepts/available_varfacs.md b/docs/src/concepts/available_varfacs.md index 1722dfd44..c2e2a3f2b 100644 --- a/docs/src/concepts/available_varfacs.md +++ b/docs/src/concepts/available_varfacs.md @@ -21,8 +21,7 @@ The variables and factors in Caesar should be sufficient for a variety of roboti Default variables in IncrementalInference ```@docs -ContinuousScalar -ContinuousEuclid{N} +Position{N} ``` ### 2D Variables @@ -40,7 +39,6 @@ DynPose2 ```@docs Point3 Pose3 -InertialPose3 ``` !!! note @@ -102,10 +100,10 @@ VelPose2VelPose2 DynPose2Pose2 Pose3Pose3 PriorPose3ZRP -PartialPriorRollPitchZ -PartialPose3XYYaw Pose3Pose3XYYaw ``` + + # Extending Caesar with New Variables and Factors diff --git a/docs/src/concepts/building_graphs.md b/docs/src/concepts/building_graphs.md index 593e825b4..5c3bf41ee 100644 --- a/docs/src/concepts/building_graphs.md +++ b/docs/src/concepts/building_graphs.md @@ -1,4 +1,4 @@ -# [Building Graphs](@ref building_graphs) +# [Building Graphs](@id 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. @@ -53,13 +53,7 @@ addVariable! deleteVariable! ``` -### Initializing Variables - -The MM-iSAMv2 algorithm uses one of two approaches to automatically initialize variables. The `initManual!` function can be used if you wish to overwrite or pre-empt this initialization. - -```@docs -initManual! -``` +The MM-iSAMv2 algorithm uses one of two approaches to automatically initialize variables, or can be [initialized manually](@ref variable_init). ## Factors diff --git a/docs/src/concepts/dataassociation.md b/docs/src/concepts/dataassociation.md index e899beda7..89fd3e79c 100644 --- a/docs/src/concepts/dataassociation.md +++ b/docs/src/concepts/dataassociation.md @@ -1,4 +1,4 @@ -# Data Association and Hypotheses +# [Data Association and Hypotheses](@id data_multihypo) Ambiguous data and processing often produce complicated data association situations. In SLAM, loop-closures are a major source of concern when developing autonomous subsystems or behaviors. To illustrate this point, consider the two scenarios depicted below: diff --git a/docs/src/concepts/entry_data.md b/docs/src/concepts/entry_data.md index 02d8fbaf6..e3a754f9b 100644 --- a/docs/src/concepts/entry_data.md +++ b/docs/src/concepts/entry_data.md @@ -140,7 +140,4 @@ addData!(dfg,:default_folder_store,:x0,:nnModel, ## Experimental Features -Loading images is a relatively common task, hence a convenience function has been developed: -```@docs -Caesar.fetchDataImage -``` +Loading images is a relatively common task, hence a convenience function has been developed, when `using ImageMagick` try `Caesar.fetchDataImage`. \ No newline at end of file diff --git a/docs/src/concepts/flux_factors.md b/docs/src/concepts/flux_factors.md index cab6bb315..7fc02aea5 100644 --- a/docs/src/concepts/flux_factors.md +++ b/docs/src/concepts/flux_factors.md @@ -1,4 +1,4 @@ # Incorporating Neural Network Factors -IncrementalInference.jl and RoME.jl has native support for using Neural Networks (via [Flux.jl](https://fluxml.ai/Flux.jl/stable/)) as non-Gaussian factors. Documentation is forthcoming, but meanwhile [see the following generic Flux.jl factor structure](https://github.com/JuliaRobotics/IncrementalInference.jl/tree/master/src/Flux). Note also that a standard [`Mixture` approach already exists too](https://github.com/JuliaRobotics/RoME.jl/blob/master/src/factors/flux/MixtureFluxPose2Pose2.jl). +IncrementalInference.jl and RoME.jl has native support for using Neural Networks (via [Flux.jl](https://fluxml.ai/Flux.jl/stable/)) as non-Gaussian factors. Documentation is forthcoming, but meanwhile [see the following generic Flux.jl factor structure](https://github.com/JuliaRobotics/IncrementalInference.jl/blob/master/ext/IncrInfrFluxFactorsExt.jl). Note also that a standard [`Mixture` approach already exists too](https://github.com/JuliaRobotics/RoME.jl/blob/master/ext/factors/MixtureFluxPose2Pose2.jl). diff --git a/docs/src/concepts/interacting_fgs.md b/docs/src/concepts/interacting_fgs.md index 68aec0978..c43eb3927 100644 --- a/docs/src/concepts/interacting_fgs.md +++ b/docs/src/concepts/interacting_fgs.md @@ -32,7 +32,7 @@ loadDFG! A later part of the documentation will show [how to include a `Entry=>Data` blob store](https://juliarobotics.org/Caesar.jl/latest/concepts/entry_data/). -## Querying the FactorGraph +## [Querying the Graph](@id querying_graph) ### List Variables: @@ -134,7 +134,7 @@ It is also possible to sample the above belief objects for more samples: pts = rand(X0, 200) ``` -## Building On-Manifold KDEs +## [Building On-Manifold KDEs](@id build_manikde) These kernel density belief objects can be constructed from points as follows: ```julia @@ -166,5 +166,5 @@ joinLogPath ```@docs getFactorDim -getManifolds +getManifold ``` \ No newline at end of file diff --git a/docs/src/concepts/multilang.md b/docs/src/concepts/multilang.md index e83b05a3f..ffd76d9eb 100644 --- a/docs/src/concepts/multilang.md +++ b/docs/src/concepts/multilang.md @@ -2,40 +2,25 @@ The Caesar framework is not limited to direct Julia use. +## [NavAbility SDKs and APIs](@id navabilitysdk) + +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. The [Documentation for the NavAbilitySDK's can be found here](https://navability.github.io/NavAbilitySDK.py/). + +Contact info@navability.io for more information. + ## 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 - 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) - -* ZMQ Interface - * 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) (needs to be updated) - * [Synchrony_py](https://github.com/nicrip/SynchronySDK_py) - -## ZMQ Messaging Interface +## [OUTDATED] 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 but rather provide a more versatile and flexible development strategy. diff --git a/docs/src/concepts/solving_graphs.md b/docs/src/concepts/solving_graphs.md index 48bb12ae3..06e16fd8c 100644 --- a/docs/src/concepts/solving_graphs.md +++ b/docs/src/concepts/solving_graphs.md @@ -14,7 +14,7 @@ The returned Bayes (Junction) `tree` object is described in more detail on [a de solveTree! ``` -## Automatic vs Manual Init +## [Automatic vs Manual Init](@id variable_init) Currently the main automatic initialization technique used by IncrementalInference.jl by delayed propagation of belief on the factor graph. This can be globally or locally controlled via: ```julia diff --git a/docs/src/concepts/using_manifolds.md b/docs/src/concepts/using_manifolds.md index b0b30f555..ea4c9eec3 100644 --- a/docs/src/concepts/using_manifolds.md +++ b/docs/src/concepts/using_manifolds.md @@ -6,7 +6,7 @@ The Community has been developing high quality [documentation for Manifolds.jl]( ## Separate Manifold Beliefs Page -Follow these hyperlinks if you are looking for information on working with Manifold Beliefs or [`ManifoldKernelDensity`s](@ref manikde_page) +See [`building a Manifold Kernel Density`](@ref build_manikde) or for more information. ## Why Manifolds.jl @@ -24,7 +24,7 @@ If you are looking for rapid help or more expertise on a particular issue, consi If you are a newcomer to the term Manifold and want to learn more, fear not even though your first search results might be somewhat disorienting. -The rest of this page is meant to introduce the basics, and point you to handy resources. Caesar.jl and [NavAbility](https://www.navability.io/) support open Community and are upstreaming improvements to Manifolds.jl, including code updates and documentation improvements. +The rest of this page is meant to introduce the basics, and point you to handy resources. Caesar.jl and [NavAbility](http://www.navability.io/) support open Community and are upstreaming improvements to Manifolds.jl, including code updates and documentation improvements. ### 'One Page' Summary of Manifolds diff --git a/docs/src/concepts/zero_install.md b/docs/src/concepts/zero_install.md index f04e8fa5c..f9895fd9e 100644 --- a/docs/src/concepts/zero_install.md +++ b/docs/src/concepts/zero_install.md @@ -1,3 +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. +See [NavAbilitySDK](@ref navabilitysdk) for details. These features will include Multi-session/agent support. diff --git a/docs/src/dev/internal_fncs.md b/docs/src/dev/internal_fncs.md index f72603a21..85793f1eb 100644 --- a/docs/src/dev/internal_fncs.md +++ b/docs/src/dev/internal_fncs.md @@ -1,5 +1,52 @@ +## Good to know + +# Conditional Multivariate Normals + +```julia +using Distributions +using LinearAlgebra + +## + +# P(A|B) + +Σab = 0.2*randn(3,3) +Σab += Σab' +Σab += diagm([1.0;1.0;1.0]) + +μ_ab = [10.0;0.0;-1.0] +μ_1 = μ_ab[1:1] +μ_2 = μ_ab[2:3] + +Σ_11 = Σab[1:1,1:1] +Σ_12 = Σab[1:1,2:3] +Σ_21 = Σab[2:3,1:1] +Σ_22 = Σab[2:3,2:3] + +## + +# P(A|B) = P(A,B) / P(B) +P_AB = MvNormal(μ_ab, Σab) # likelihood +P_B = MvNormal([-0.5;0.75], [0.75 0.3; 0.3 2.0]) # evidence + +# Schur compliment +μ_(b) = μ_1 + Σ_12*Σ_22^(-1)*(b-μ_2) +Σ_ = Σ_11 + Σ_12*Σ_22^(-1)*Σ_21 + +P_AB_B(a,b) = pdf(P_AB, [a;b]) / pdf(P_B, b) +P_A_B(a,b; mv = MvNormal(μ_(b), Σ_)) = pdf(mv, a) + +## + +# probability density: p(a) = P(A=a | B=b) +@show P_A_B([1.;],[0.;0.]) +@show P_AB_B([1.;],[0.;0.]) + +P(A|B=B(.)) +``` + # Various Internal Function Docs ```@docs _solveCCWNumeric! -``` \ No newline at end of file +``` diff --git a/docs/src/dev/known_issues.md b/docs/src/dev/known_issues.md index 2eb4e95b3..60ef09e03 100644 --- a/docs/src/dev/known_issues.md +++ b/docs/src/dev/known_issues.md @@ -10,7 +10,7 @@ This page is used to list known issues: ### 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. +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). diff --git a/docs/src/examples/adding_variables_factors.md b/docs/src/examples/adding_variables_factors.md index cb0573fa5..774133eb6 100644 --- a/docs/src/examples/adding_variables_factors.md +++ b/docs/src/examples/adding_variables_factors.md @@ -4,7 +4,7 @@ A couple of important points: * You **do not need to** modify or insert your new code into Caesar/RoME/IncrementalInference source code libraries -- they can be created and run anywhere on-the-fly! * As long as the factors exist in the working space when the solver is run, the factors are automatically used -- this is possible due to Julia's [multiple dispatch design](https://docs.julialang.org/en/v1/manual/methods/index.html) * Caesar.jl is designed to allow you to add new variables and factors to your own independent repository and incorporate them at will at compile-time or even run-time -* Residual function definitions for new factors types use a [callable struct (a.k.a functor) architecture](@ref custom_residual_factor) to simultaneously allow: +* Residual function definitions for new factors types use a [callable struct (a.k.a functor) architecture](@ref custom_relative_factor) to simultaneously allow: * Multiple dispatch (i.e. 'polymorphic' behavior) * Meta-data and in-place memory storage for advanced and performant code * An outside callback implementation style @@ -17,13 +17,12 @@ A couple of important points: All factors inherit from one of the following types, depending on their function: -* AbstractPrior: AbstractPrior are priors (unary factors) that provide an absolute constraint for a single variable. A simple example of this is an absolute GPS prior, or equivalently a (0, 0, 0) starting location in a [`Pose2`](@ref) scenario. - * Requires: A getSample function -* IIF.AbstractRelativeMinimize: IIF.AbstractRelativeMinimize are relative factors that introduce an algebraic relationship between two or more variables. A simple example of this is an odometry factor between two pose variables, or a range factor indicating the range between a pose and another variable. - * Requires: A getSample function and a residual function definition +* `AbstractPrior` is for priors (unary factors) that provide an absolute constraint for a single variable. A simple example of this is an absolute GPS prior, or equivalently a (0, 0, 0) starting location in a [`Pose2`](@ref) scenario. + * Requires: A `getSample` function +* `AbstractRelativeMinimize` uses Optim.jl and is for relative factors that introduce an algebraic relationship between two or more variables. A simple example of this is an odometry factor between two pose variables, or a range factor indicating the range between a pose and another variable. + * Requires: A `getSample` function and a residual function definition * The minimize suffix specifies that the residual function of this factor will be enforced by numerical minimization (find me the minimum of this function) -* IIF.AbstractRelativeRoots: IIF.AbstractRelativeRoots are relative factors that introduce algebraic relationships between two or more variables. They are the same as IIF.AbstractRelativeMinimize, however they use root finding to find the zero crossings (rather than numerical minimization). - * Requires: A getSample function and a residual function definition +* [NEW] `AbstractManifoldMinimize` uses [Manopt.jl](https://github.com/JuliaManifolds/Manopt.jl). How do you decide which to use? * If you are creating factors for world-frame information that will be tied to a single variable, inherit from `<:AbstractPrior` @@ -33,16 +32,14 @@ How do you decide which to use? TBD: Users should start with IIF.AbstractRelativeMinimize, discuss why and when they should promote their factors to IIF.AbstractRelativeRoots. !!! note - `IIF.AbstractRelativeMinimize` does not imply that the overall inference algorithm only minimizes an objective function. The MM-iSAM algorithm is built around fixed-point analysis. Minimization is used here to locally enforce the residual function. + `AbstractRelativeMinimize` does not imply that the overall inference algorithm only minimizes an objective function. The MM-iSAM algorithm is built around fixed-point analysis. Minimization is used here to locally enforce the residual function. What you need to build in the new factor: * A struct for the factor itself * A sampler function to return measurements from the random ditributions -* If you are building a [`IIF.AbstractRelativeMinimize`](@ref) or a [`IIF.AbstractRelativeRoots`](@ref) you need to define a residual function to introduce the relative algebraic relationship between the variables +* If you are building a `<:AbstractRelative` you need to define a residual function to introduce the relative algebraic relationship between the variables * Minimization function should be lower-bounded and smooth * A packed type of the factor which must be named Packed[Factor name], and allows the factor to be packed/transmitted/unpacked * Serialization and deserialization methods * These are convert functions that pack and unpack the factor (which may be highly complex) into serialization-compatible formats - * As the factors are mostly comprised of distributions (of type [`SamplableBelief`](@ref)), functions are provided to pack and unpack the distributions: - * Packing: To convert from a [`SamplableBelief`](@ref) to a serializable obhect, use `convert(PackedSamplableBelief, ::SamplableBelief)` - * Unpacking: To convert from string back to a `SamplableBelief`, use `convert(SamplableBelief, ::PackedSamplableBelief)` + * As the factors are mostly comprised of distributions (of type `SampleableBelief`), while `JSON3.jl`` is used for serialization. diff --git a/docs/src/examples/basic_definingfactors.md b/docs/src/examples/basic_definingfactors.md index c461a3912..02549acf6 100644 --- a/docs/src/examples/basic_definingfactors.md +++ b/docs/src/examples/basic_definingfactors.md @@ -49,8 +49,8 @@ This new prior can now readily be added to an ongoing factor graph: ```julia # lets generate a random nonparametric belief -pts = [samplePoint(getManifold(ContinuousEuclid{1}), Normal(8.0,2.0)) for _=1:75] -someBelief = manikde!(ContinuousEuclid{1}, pts) +pts = [samplePoint(getManifold(Position{1}), Normal(8.0,2.0)) for _=1:75] +someBelief = manikde!(Position{1}, pts) # and build your new factor as an object myprior = MyPrior(someBelief) @@ -61,7 +61,10 @@ and add it to the existing factor graph from earlier, lets say: addFactor!(fg, [:x1], myprior) ``` -Thats it, this factor is now part of the graph. This should be a solvable graph: +!!! note + Variable types `Postion{1}` or `ContinuousEuclid{1}` are algebraically equivalent. + +That's it, this factor is now part of the graph. This should be a solvable graph: ```julia solveGraph!(fg); # exact alias of solveTree!(fg) ``` diff --git a/docs/src/examples/canonical_graphs.md b/docs/src/examples/canonical_graphs.md index 2ff9f35a5..a6336ba9a 100644 --- a/docs/src/examples/canonical_graphs.md +++ b/docs/src/examples/canonical_graphs.md @@ -11,8 +11,8 @@ 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/deadreckontether.md b/docs/src/examples/deadreckontether.md index 49cfae014..373258e96 100644 --- a/docs/src/examples/deadreckontether.md +++ b/docs/src/examples/deadreckontether.md @@ -20,7 +20,7 @@ Overview of related functions while this documentation is being expanded: ## DRT Construct -The idea is that the dead reckong tracking method is to update a single value based on high-rate sensor data. Perhaps 'particles' values can be propagated as a non-Gaussian prediction, depending on allowable compute resources, and for that see [`approxConv`](@ref). Some specialized plumbing has been built to facilitate rapid single value propagation using the factor graph. +The idea is that the dead reckong tracking method is to update a single value based on high-rate sensor data. Perhaps 'particles' values can be propagated as a non-Gaussian prediction, depending on allowable compute resources, and for that see [`approxConvBelief`](@ref). Some specialized plumbing has been built to facilitate rapid single value propagation using the factor graph. ### Suppress w/ `solvable` diff --git a/docs/src/examples/examples.md b/docs/src/examples/examples.md index d16c263ff..83685eb2a 100644 --- a/docs/src/examples/examples.md +++ b/docs/src/examples/examples.md @@ -71,11 +71,11 @@ This example presents a novel multimodal solution to an otherwise intractible mu
``` -The fractional multi-hypothesis assignments `addFactor!(..., multihypo=[1.0; 0.5;0.5])`. The [Multihypothesis](@ref) Section discusses this feature in more detail. Similarly for tri-nary or higher multi-hypotheses. +The fractional multi-hypothesis assignments `addFactor!(..., multihypo=[1.0; 0.5;0.5])`. Similarly for tri-nary or higher multi-hypotheses. ### Probabilistic Data Association (Uncertain loop closures) -Example where the standard multihypothesis `addFactor!(.., multihypo=[1.0;0.5;0.5])` interface is used. This is from the Kitti driving dataset. [Video here](https://www.youtube.com/watch?v=9hEonD8KDrs). The [Multihypothesis](@ref) Section discusses this feature in more detail. +Example where the standard multihypothesis `addFactor!(.., multihypo=[1.0;0.5;0.5])` interface is used. This is from the Kitti driving dataset. [Video here](https://www.youtube.com/watch?v=9hEonD8KDrs). The [data association and multihypothesis](@ref data_multihypo) section discusses this feature in more detail. ```@raw html diff --git a/docs/src/examples/parametric_solve.md b/docs/src/examples/parametric_solve.md index c3f36668d..1f37729ee 100644 --- a/docs/src/examples/parametric_solve.md +++ b/docs/src/examples/parametric_solve.md @@ -21,15 +21,9 @@ initParametricFrom! Factor that supports a parametric solution, with supported distributions (such as `Normal` and `MvNormal`), can be used in a parametric batch solver `solveGraphParametric`. -### `getParametricMeasurement` +### `getMeasurementParametric` -Parameteric calculations require the mean and covariance from Gaussian measurement functions (factors) using the function - -```@docs -IncrementalInference.getParametricMeasurement -``` - -`getParametricMeasurement` defaults to looking for a supported distribution in field `.Z` followed by `.z`. Therefore, if the factor uses this fieldname, `getParametricMeasurement` does not need to be extended. You can extend by simply implementing, for example, your own `IncrementalInference.getParametricMeasurement(f::OtherFactor) = m.density`. +Parameteric calculations require the mean and covariance from Gaussian measurement functions (factors) using the `getMeasurementParametric` `getMeasurementParametric` defaults to looking for a supported distribution in field `.Z` followed by `.z`. Therefore, if the factor uses this fieldname, `getMeasurementParametric` does not need to be extended. You can extend by simply implementing, for example, your own `IncrementalInference.getMeasurementParametric(f::OtherFactor) = m.density`. For this example, the `Z` field will automatically be detected used by default for `MyFactor` from above. @@ -39,10 +33,10 @@ struct MyFactor{T <: SamplableBelief} <: IIF.AbstractRelativeRoots end ``` -An example of where implementing `getParametricMeasurement` is needed can be found in the RoME factor [`Pose2Point2BearingRange`](@ref) +An example of where implementing `getMeasurementParametric` is needed can be found in the RoME factor [`Pose2Point2BearingRange`](@ref) ```julia -import getParametricMeasurement -function getParametricMeasurement(s::Pose2Point2BearingRange{<:Normal, <:Normal}) +import getMeasurementParametric +function getMeasurementParametric(s::Pose2Point2BearingRange{<:Normal, <:Normal}) meas = [mean(s.bearing), mean(s.range)] iΣ = [1/var(s.bearing) 0; diff --git a/docs/src/examples/using_pcl.md b/docs/src/examples/using_pcl.md index 773538e35..87ae219a3 100644 --- a/docs/src/examples/using_pcl.md +++ b/docs/src/examples/using_pcl.md @@ -3,14 +3,14 @@ ## Introduction `Caesar._PCL` A wide ranging and well used [point cloud library exists called PCL](https://pointclouds.org/) which is implemented in C++. To get access to many of those features and bridge the Caesar.jl suite of packages, the base `PCL.PointCloud` types have been implemented in Julia and reside under `Caesar._PCL`. The main types of interest: -- [`Caesar._PCL.PointCloud`](@ref), -- [`Caesar._PCL.PCLPointCloud2`](@ref), -- [`Caesar._PCL.PointXYZ`](@ref), -- [`Caesar._PCL.Header`](@ref), -- [`Caesar._PCL.PointField`](@ref), -- [`Caesar._PCL.FieldMapper`](@ref). - -These types are conditionally loaded once `Colors.jl` is included (see [Requires.jl](https://github.com/JuliaPackaging/Requires.jl)), +- `Caesar._PCL.PointCloud` +- `Caesar._PCL.PCLPointCloud2` +- `Caesar._PCL.PointXYZ` +- `Caesar._PCL.Header` +- `Caesar._PCL.PointField` +- `Caesar._PCL.FieldMapper` + +The PointCloud types use `Colors.jl`: ```julia using Colors, Caesar using StaticArrays @@ -23,7 +23,7 @@ pt = Caesar._PCL.PointXYZ(;data=SA[x,y,z,intens]) ``` ```@docs -Caesar._PCL.PointCloud +_PCL.PointCloud ``` ## Conversion with `ROS.PointCloud2` @@ -34,11 +34,11 @@ Strong integration between PCL and [ROS](http://www.ros.org) predominantly throu These have been integrated through conversions to equivalent Julian types already listed above. ROS conversions requires RobotOS.jl be loaded, see page on using [ROS Direct](@ref ros_direct). ```@docs -Caesar._PCL.PCLPointCloud2 -Caesar._PCL.PointXYZ -Caesar._PCL.Header -Caesar._PCL.PointField -Caesar._PCL.FieldMapper +_PCL.PointXYZ +_PCL.Header +_PCL.PointField +_PCL.FieldMapper +_PCL.PCLPointCloud2 ``` ## Aligning Point Clouds diff --git a/docs/src/func_ref.md b/docs/src/func_ref.md index 5eddda071..7de656e0a 100644 --- a/docs/src/func_ref.md +++ b/docs/src/func_ref.md @@ -22,17 +22,14 @@ areCliqVariablesAllMarginalized attemptTreeSimilarClique childCliqs cliqHistFilterTransitions -csmAnimate cycleInitByVarOrder! doautoinit! drawCliqSubgraphUpMocking fifoFreeze! filterHistAllToArray -findRelatedFromPotential fmcmc! getClique getCliqAllVarIds -getCliqAllVarSyms getCliqAssocMat getCliqDepth getCliqDownMsgsAfterDownSolve @@ -45,8 +42,6 @@ getCliqVarIdsPriors getCliqVarSingletons getParent getTreeAllFrontalSyms -getVariableDim -getVariableInferredDim hasClique isInitialized isMarginalized @@ -57,19 +52,13 @@ makeCsmMovie parentCliq predictVariableByFactor printCliqHistorySummary -prodmultiplefullpartials -prodmultipleonefullpartials -resetBuildTreeFromOrder! resetCliqSolve! resetData! resetTreeCliquesForUpSolve! resetVariable! -sandboxCliqResolveStep setfreeze! setValKDE! setVariableInitialized! -setVariableInferDim! -solveCliq! solveCliqWithStateMachine! transferUpdateSubGraph! treeProductDwn diff --git a/docs/src/index.md b/docs/src/index.md index 2e0ce1c33..71704e341 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -14,7 +14,7 @@ Caesar.jl is a community project to facilate software technology development for # Commercial Products and Services -[NavAbility.io](http://www.navability.io) builds [products and services](https://www.navability.io/products/) which 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. +[WhereWhen.ai's NavAbility products and services](https://www.wherewhen.ai) build upon, continually develop, and help administer the Caesar.jl suite of open-source libraries. Please reach out for any additional information (info@navability.io), or using the community links provided below. Various mapping and localization solutions are possible both for commercial and R&D. We recommend taking a look at: - The human-to-machine friendly [NavAbility App](https://app.navability.io/home/) interaction; and diff --git a/docs/src/introduction.md b/docs/src/introduction.md index 6193dbd43..a2adfcd0b 100644 --- a/docs/src/introduction.md +++ b/docs/src/introduction.md @@ -13,14 +13,14 @@ Caesar.jl addresses numerous issues that arise in prior SLAM solutions, includin * 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): + * Native multi-modal (hypothesis) representation in the factor-graph, see [Data Association and Hypotheses](@ref data_multihypo): * 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); +* Out-of-library extendable for [Custom New Variables](@ref custom_variables) and [Factors](@ref custom_prior_factor); * 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); diff --git a/docs/src/principles/bayestreePrinciples.md b/docs/src/principles/bayestreePrinciples.md index c444785c9..3d7d488c6 100644 --- a/docs/src/principles/bayestreePrinciples.md +++ b/docs/src/principles/bayestreePrinciples.md @@ -64,7 +64,7 @@ buildTreeReset!(fg, vo) ``` !!! note - a list of variables or factors can be obtained through the `ls` and related functions, see [Querying the FactorGraph](@ref). + a list of variables or factors can be obtained through the `ls` and related functions, see [Querying the Factor Graph](@ref querying_graph). ## Interfacing with the MM-iSAMv2 Solver @@ -79,7 +79,7 @@ tree = solveTree!(fg) ``` !!! note - See the [Solving Graphs](@ref) section for more details on the solver. + See the [Solving Graphs](@ref solving_graphs) section for more details on the solver. ### Get the Elimination Order Used diff --git a/examples/marine/auv/Sandshark/MakiePlotsFG.jl b/examples/marine/auv/Sandshark/MakiePlotsFG.jl index 3c4172318..8a2ad725e 100644 --- a/examples/marine/auv/Sandshark/MakiePlotsFG.jl +++ b/examples/marine/auv/Sandshark/MakiePlotsFG.jl @@ -14,56 +14,8 @@ using Makie using MakieLayout using DocStringExtensions +import Arena: getRangeCartesian -""" - $SIGNATURES - -Get the cartesian range over which the factor graph variables span. - -Notes: -- Optional `regexFilter` can be used to subselect, according to label, which variable IDs to use. - -DevNotes -- TODO, allow `tags` as filter too. -""" -function getRangeCartesian(dfg::AbstractDFG, - regexFilter::Union{Nothing, Regex}=nothing; - extend::Float64=0.2, - digits::Int=6, - xmin::Real=99999999, - xmax::Real=-99999999, - ymin::Real=99999999, - ymax::Real=-99999999, - force::Bool=false ) - # - - if !force - # which variables to consider - vsyms = getVariableIds(dfg, regexFilter) - - # find the cartesian range over all the vsyms variables - - for vsym in vsyms - lran = getKDERange(getVariable(dfg, vsym) |> getKDE) - xmin = lran[1,1] < xmin ? lran[1,1] : xmin - ymin = lran[2,1] < ymin ? lran[2,1] : ymin - xmax = xmax < lran[1,2] ? lran[1,2] : xmax - ymax = ymax < lran[2,2] ? lran[2,2] : ymax - end - - # extend the range for looser bounds on plot - xra = xmax-xmin; xra *= extend - yra = ymax-ymin; yra *= extend - xmin -= extend; xmax += extend - ymin -= extend; ymax += extend - - # clamp to nearest integers - xmin = floor(xmin, digits=digits); xmax = ceil(xmax, digits=digits) - ymin = floor(ymin, digits=digits); ymax = ceil(ymax, digits=digits) - end - - return [xmin xmax; ymin ymax] -end """ diff --git a/ext/Pose2AprilTag4Corners.jl b/ext/Pose2AprilTag4Corners.jl index 39ef66040..99226755f 100644 --- a/ext/Pose2AprilTag4Corners.jl +++ b/ext/Pose2AprilTag4Corners.jl @@ -258,28 +258,6 @@ end ## calibrate via preimage -""" - $SIGNATURES - -Helper function to generate and calculate the aggregate cost indicative of the discrepancy between -the deconvolution prediction and prescribed measurement of mutliple `Pose2AprilTag4Corners` factors -in the factor graph. - -The idea is that a bad calibration will give some kind of SLAM result, and if there is enough information -then the SLAM result can be used to bootstrap better and better calibration estimates of the camera that -was used to capture the AprilTag sightings. This function is meant to help do that secondary parameter -search inside a factor graph objection, after a regular solution has been found. - -Notes -- `pred, _ = approxDeconv(dfg, fct)` -- `fct.preimage[1](pred[:,idx], [f_width, f_height, c_width, c_height, taglength])` - - `fct.preimage[1]` is a function to find the preimage. -- `obj = (fc_wh) -> fct.preimage[1](pred[:,idx], fc_wh)` - - `fc_wh = [f_width, f_height, c_width, c_height, 0.172]` -- `obj2 = (fcwh) -> obj([fcwh[1]; fcwh[1]; fcwh[2]; c_height; taglength])` -- `result = Optim.optimize(obj, fct.preimage[2], BFGS(), Optim.options(x_tol=1e-8))` - - A stored starting estimate for optimization `fct.preimage[2]` -""" function generateCostAprilTagsPreimageCalib(dfg::AbstractDFG, fsyms::Vector{Symbol}=lsf(dfg, Pose2AprilTag4Corners ); idx::Int = 1, # the sample number diff --git a/ext/WeakdepsPrototypes.jl b/ext/WeakdepsPrototypes.jl index 36ea02fcd..8fd9df17a 100644 --- a/ext/WeakdepsPrototypes.jl +++ b/ext/WeakdepsPrototypes.jl @@ -6,6 +6,29 @@ export Pose2AprilTag4Corners, PackedPose2AprilTag4Corners export generateCostAprilTagsPreimageCalib function drawBearingLinesAprilTags! end + +""" + $SIGNATURES + +Helper function to generate and calculate the aggregate cost indicative of the discrepancy between +the deconvolution prediction and prescribed measurement of mutliple `Pose2AprilTag4Corners` factors +in the factor graph. + +The idea is that a bad calibration will give some kind of SLAM result, and if there is enough information +then the SLAM result can be used to bootstrap better and better calibration estimates of the camera that +was used to capture the AprilTag sightings. This function is meant to help do that secondary parameter +search inside a factor graph objection, after a regular solution has been found. + +Notes +- `pred, _ = approxDeconv(dfg, fct)` +- `fct.preimage[1](pred[:,idx], [f_width, f_height, c_width, c_height, taglength])` + - `fct.preimage[1]` is a function to find the preimage. +- `obj = (fc_wh) -> fct.preimage[1](pred[:,idx], fc_wh)` + - `fc_wh = [f_width, f_height, c_width, c_height, 0.172]` +- `obj2 = (fcwh) -> obj([fcwh[1]; fcwh[1]; fcwh[2]; c_height; taglength])` +- `result = Optim.optimize(obj, fct.preimage[2], BFGS(), Optim.options(x_tol=1e-8))` + - A stored starting estimate for optimization `fct.preimage[2]` +""" function generateCostAprilTagsPreimageCalib end ## ============================================== diff --git a/ext/services/LasIOSupport.jl b/ext/services/LasIOSupport.jl index 20ab56e41..4de18a5ae 100644 --- a/ext/services/LasIOSupport.jl +++ b/ext/services/LasIOSupport.jl @@ -44,7 +44,7 @@ function saveLAS( r = map(p->p.color.r, pc.points) g = map(p->p.color.g, pc.points) b = map(p->p.color.b, pc.points) - pCloud = LasPoint2.(round.(scale*x),round.(scale*y),round.(scale*z), 1, 0, 0, 0, 0, 0, r, g, b); + pCloud = LasPoint2.(round.(Int32,scale*x),round.(Int32,scale*y),round.(Int32,scale*z), 1, 0, 0, 0, 0, 0, r, g, b); tim = unix2datetime(pc.header.stamp*1e-6) # now() diff --git a/src/3rdParty/_PCL/entities/PCLTypes.jl b/src/3rdParty/_PCL/entities/PCLTypes.jl index f03fc1f15..f47bc5ea5 100644 --- a/src/3rdParty/_PCL/entities/PCLTypes.jl +++ b/src/3rdParty/_PCL/entities/PCLTypes.jl @@ -134,7 +134,12 @@ Base.@kwdef struct Header frame_id::String = "" end -# https://pointclouds.org/documentation/structpcl_1_1_p_c_l_point_field.html +""" + $TYPEDEF + +How a point is stored in memory. +- https://pointclouds.org/documentation/structpcl_1_1_p_c_l_point_field.html +""" Base.@kwdef struct PointField """ name of field """ name::String @@ -165,7 +170,13 @@ end # 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 +""" +$TYPEDEF + +Which field values to store and how to map them to values during serialization. + +- 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}() @@ -182,7 +193,7 @@ References: - https://pointclouds.org/documentation/classpcl_1_1_point_cloud.html - https://pointclouds.org/documentation/common_2include_2pcl_2point__cloud_8h_source.html -See also: [`Caesar._PCL.toROSPointCloud2`](@ref) +See also: `Caesar._PCL.toROSPointCloud2` """ Base.@kwdef struct PCLPointCloud2 """ the point cloud header """ diff --git a/src/images/ROSConversions.jl b/src/images/ROSConversions.jl index 397d272de..e061ab446 100644 --- a/src/images/ROSConversions.jl +++ b/src/images/ROSConversions.jl @@ -32,7 +32,7 @@ toImage(msg::Main.sensor_msgs.msg.Image) = unmarshal(msg) |> toImage Convert `Caesar.Image::Dict` type to ROS message `sensor_msgs.msg.Image`. -See also: [`Caesar.unmarshal`](@ref), [`Caesar.toImage`](@ref), [`Caesar._PCL.toROSPointCloud2`](@ref) +See also: [`Caesar.unmarshal`](@ref), [`Caesar.toImage`](@ref), `Caesar._PCL.toROSPointCloud2` """ function toROSImage(msgd::Dict{String,Any}) header = Main.std_msgs.msg.Header(); diff --git a/test/ICRA2022_tests/nongaussian_mixture.jl b/test/ICRA2022_tests/nongaussian_mixture.jl index 2077500ff..dfed891c1 100644 --- a/test/ICRA2022_tests/nongaussian_mixture.jl +++ b/test/ICRA2022_tests/nongaussian_mixture.jl @@ -58,9 +58,9 @@ solveGraph!(fg); X2 = getBelief(fg, :x2) -@test isapprox( X2([-50:0.1:0;])*0.1 |> sum, 0 ; atol=0.1 ) -@test isapprox( X2([0:0.1:40;])*0.1 |> sum, 0.4; atol=0.2 ) # 40% case -@test isapprox( X2([40:0.1:80;])*0.1 |> sum, 0.6; atol=0.2 ) # 60% case +@test isapprox( sum((s->X2([s;])).([-50:0.1:0;])*0.1)[1], 0 ; atol=0.1 ) +@test isapprox( sum((s->X2([s;])).([0:0.1:40;])*0.1)[1], 0.4; atol=0.2 ) # 40% case +@test isapprox( sum((s->X2([s;])).([40:0.1:80;])*0.1)[1], 0.6; atol=0.2 ) # 60% case # plotBelief(fg, [:x0, :x1, :x2]) @@ -75,14 +75,14 @@ solveGraph!(fg); X2 = getBelief(fg, :x2) -@test isapprox( X2([-50:0.1:0;])*0.1 |> sum, 0 ; atol=0.1 ) -@test isapprox( X2([0:0.1:40;])*0.1 |> sum, 0.4; atol=0.2 ) # 40% case -@test isapprox( X2([40:0.1:80;])*0.1 |> sum, 0.6; atol=0.2 ) # 60% case +@test isapprox( sum((s->X2([s;])).([-50:0.1:0;])*0.1)[1], 0 ; atol=0.1 ) +@test isapprox( sum((s->X2([s;])).([0:0.1:40;])*0.1)[1], 0.4; atol=0.2 ) # 40% case +@test isapprox( sum((s->X2([s;])).([40:0.1:80;])*0.1)[1], 0.6; atol=0.2 ) # 60% case X3 = getBelief(fg, :x3) -@test isapprox( X3([-100:0.1:-50;])*0.1 |> sum, 0 ; atol=0.1 ) -@test isapprox( X3([-50:0.1:-20;] )*0.1 |> sum, 0.4; atol=0.2 ) # 40% case -@test isapprox( X3([-10:0.1:30;] )*0.1 |> sum, 0.6; atol=0.2 ) # 60% case +@test isapprox( sum((s->X3([s;])).([-100:0.1:-50;])*0.1)[1], 0 ; atol=0.1 ) +@test isapprox( sum((s->X3([s;])).([-50:0.1:-20;] )*0.1)[1], 0.4; atol=0.2 ) # 40% case +@test isapprox( sum((s->X3([s;])).([-10:0.1:30;] )*0.1)[1], 0.6; atol=0.2 ) # 60% case addFactor!(fg, [:x3, :x0], LinearRelative(Normal(30, 1)))