From 34dee9e6d8cf36a8a1a9ee9221816b79401e5a74 Mon Sep 17 00:00:00 2001 From: CompatHelper Julia Date: Mon, 25 Jul 2022 22:19:50 +0000 Subject: [PATCH 1/5] CompatHelper: add new compat entry for StatsBase at version 0.33, (keep existing compat) --- Project.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 81a2960a1..a0d3e10f7 100644 --- a/Project.toml +++ b/Project.toml @@ -26,8 +26,8 @@ JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3" KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" -NearestNeighbors = "b8a86587-4115-5ab1-83bc-aa920d37bbce" NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56" +NearestNeighbors = "b8a86587-4115-5ab1-83bc-aa920d37bbce" Optim = "429524aa-4258-5aef-a3af-852621145aeb" Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7" @@ -73,6 +73,7 @@ Requires = "1" RoME = "0.20" Rotations = "1.1" StaticArrays = "1" +StatsBase = "0.33" TensorCast = "0.4" TimeZones = "1.3.1, 1.4" TransformUtils = "0.2.14" From 6edddd8629866a65764a422f09c0474e72437c9a Mon Sep 17 00:00:00 2001 From: CompatHelper Julia Date: Mon, 25 Jul 2022 22:19:57 +0000 Subject: [PATCH 2/5] CompatHelper: add new compat entry for NearestNeighbors at version 0.4, (keep existing compat) --- Project.toml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 81a2960a1..f9b250dca 100644 --- a/Project.toml +++ b/Project.toml @@ -26,8 +26,8 @@ JSON2 = "2535ab7d-5cd8-5a07-80ac-9b1792aadce3" KernelDensityEstimate = "2472808a-b354-52ea-a80e-1658a3c6056d" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e" -NearestNeighbors = "b8a86587-4115-5ab1-83bc-aa920d37bbce" NLsolve = "2774e3e8-f4cf-5e23-947b-6d7e65073b56" +NearestNeighbors = "b8a86587-4115-5ab1-83bc-aa920d37bbce" Optim = "429524aa-4258-5aef-a3af-852621145aeb" Pkg = "44cfe95a-1eb2-52ea-b672-e2afdf69b78f" Printf = "de0858da-6303-5e67-8744-51eddeeeb8d7" @@ -66,6 +66,7 @@ JSON2 = "0.3, 0.4" KernelDensityEstimate = "0.5" Manifolds = "0.8" NLsolve = "4" +NearestNeighbors = "0.4" Optim = "1" ProgressMeter = "1" Reexport = "1" From 662da9906105fb5b5f2b0251a12c489cc5120cb2 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 25 Jul 2022 15:36:13 -0700 Subject: [PATCH 3/5] add docs for initVariable! --- docs/src/concepts/solving_graphs.md | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/docs/src/concepts/solving_graphs.md b/docs/src/concepts/solving_graphs.md index 3b5726c1c..650f4f595 100644 --- a/docs/src/concepts/solving_graphs.md +++ b/docs/src/concepts/solving_graphs.md @@ -1,10 +1,11 @@ # [Solving Graphs](@id solving_graphs) +## Non-parametric Batch Solve When you have built the graph, you can call the solver to perform inference with the following: ```julia # Perform inference -tree = solveTree!(fg) +tree = solveTree!(fg) # or solveGraph! ``` 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/`). @@ -13,6 +14,21 @@ The returned Bayes (Junction) `tree` object is described in more detail on [a de solveTree! ``` +## Automatic vs Manual 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 +getSolverParams(fg).graphinit = false + +# or locally at each addFactor +addFactor!(fg, [:x0;:x1], LinearRelative(Normal()); graphinit=false) +``` + +Use [`initVariable!`](@ref) if you'd like to force a particular numerical initialization of some or all the variables. +```@docs +initVariable! +``` + ## 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): From 364cdb78773c76ab9042e6026a93e7656de9b19b Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 25 Jul 2022 15:45:48 -0700 Subject: [PATCH 4/5] additional docs fixes --- docs/src/concepts/solving_graphs.md | 5 +++++ src/3rdParty/_PCL/services/ICP_Simple.jl | 2 +- src/images/ScatterAlignPose2.jl | 4 +++- 3 files changed, 9 insertions(+), 2 deletions(-) diff --git a/docs/src/concepts/solving_graphs.md b/docs/src/concepts/solving_graphs.md index 650f4f595..48bb12ae3 100644 --- a/docs/src/concepts/solving_graphs.md +++ b/docs/src/concepts/solving_graphs.md @@ -29,6 +29,11 @@ Use [`initVariable!`](@ref) if you'd like to force a particular numerical initia initVariable! ``` +All the variables can be initialized without solving with: +```@docs +initAll! +``` + ## 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): diff --git a/src/3rdParty/_PCL/services/ICP_Simple.jl b/src/3rdParty/_PCL/services/ICP_Simple.jl index 2c8647d6c..7d2585581 100644 --- a/src/3rdParty/_PCL/services/ICP_Simple.jl +++ b/src/3rdParty/_PCL/services/ICP_Simple.jl @@ -231,7 +231,7 @@ Notes - See here for a brief example on [Visualizing Point Clouds](@ref viz_pointcloud_makie). DevNotes -- TODO switch rigid transfrom to `Caesar._PCL.apply` along with performance considerations, instead of curent `transform!`. +- TODO switch rigid transfrom to `Caesar._PCL.apply` along with performance considerations, instead of current `transform!`. See also: [`PointCloud`](@ref) """ diff --git a/src/images/ScatterAlignPose2.jl b/src/images/ScatterAlignPose2.jl index 77e608587..448933c76 100644 --- a/src/images/ScatterAlignPose2.jl +++ b/src/images/ScatterAlignPose2.jl @@ -92,7 +92,9 @@ struct ScatterAlignPose2 <: IIF.AbstractManifoldMinimize end """ - ScatterAlignPose3(; mkd1::ManifoldKernelDensity, mkd2::ManifoldKernelDensity, moreoptions...) + ScatterAlignPose3(; cloud1=mkd1::ManifoldKernelDensity, + cloud2=mkd2::ManifoldKernelDensity, + moreoptions...) Specialization of [`ScatterAlign`](@ref) for [`Pose3`](@ref). From 78c37a38a2ce1f2ef4bd003a5bb00e4d253e8bb9 Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 25 Jul 2022 16:06:01 -0700 Subject: [PATCH 5/5] fix test --- test/pcl/testPointCloud2.jl | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/test/pcl/testPointCloud2.jl b/test/pcl/testPointCloud2.jl index 082d96e90..b8e3891a3 100644 --- a/test/pcl/testPointCloud2.jl +++ b/test/pcl/testPointCloud2.jl @@ -135,6 +135,8 @@ pc_ = Caesar._PCL.apply(M, rPc, pc) end +if v"1.7" <= VERSION + @testset "PandarXT test point cloud conversion test" begin ## @@ -220,6 +222,11 @@ pc_3D = Caesar._PCL.apply(M, rPc, pc) ## end +else + @error "Skip PointCloud testdata serialization owning to Julia version." + @test_broken false +end + @testset "Test PointCloud on Terrestrial Lidar and ICP_Simple alignment" begin