From 3871fb0445174dc12883bacdefd3d24c9f33b79b Mon Sep 17 00:00:00 2001 From: Johannes Terblanche Date: Tue, 15 Sep 2020 12:30:53 +0200 Subject: [PATCH 1/7] bump to v0.9.1 --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 090db9f7..b993685d 100644 --- a/Project.toml +++ b/Project.toml @@ -2,7 +2,7 @@ name = "RoME" uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8" keywords = ["SLAM", "state-estimation", "MM-iSAM", "inference", "robotics"] desc = "Non-Gaussian simultaneous localization and mapping" -version = "0.9.0" +version = "0.9.1" [deps] ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89" From f1b07372dd0a3b191f872c0a3d7f85323d3aed30 Mon Sep 17 00:00:00 2001 From: dehann Date: Sat, 19 Sep 2020 21:58:14 -0400 Subject: [PATCH 2/7] iif v0.16.0 compat --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index b993685d..42ef4226 100644 --- a/Project.toml +++ b/Project.toml @@ -37,7 +37,7 @@ Distributions = "0.20, 0.21, 0.22, 0.23" DocStringExtensions = "0.7, 0.8, 0.9, 0.10, 1" FileIO = "1.0.2, 1.1, 1.2" Graphs = "0.10.2, 0.11, 1" -IncrementalInference = "0.14.1, 0.15" +IncrementalInference = "0.14.1, 0.15, 0.16" JLD2 = "0.1, 0.2, 0.3, 1.0" KernelDensityEstimate = "0.5.1, 0.6" Optim = "0.19, 0.20, 0.21, 0.22, 1.0" From e6293332f3723d9c37ea179f5c3ec920ac9cf6e4 Mon Sep 17 00:00:00 2001 From: Dehann Fourie Date: Sat, 19 Sep 2020 23:27:55 -0400 Subject: [PATCH 3/7] Update README.md --- README.md | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/README.md b/README.md index 779a1fd7..6c880dba 100644 --- a/README.md +++ b/README.md @@ -1,3 +1,11 @@ +# NOTICE Gadfly#master + +Note current installation requires the latest Gadfly to overcome a Type Piracy problem that occurred in ImageCore.jl. The following installation will fix the recent (but transient) Pkg compat problems: +```julia +julia> ] # to activate package manager +(@v1.5) pkg> add Gadfly#master +(@v1.5) pkg> add IncrementalInference RoME Caesar + # RoME.jl | Stable | Dev | Coverage | From fffbdc8deb18dce5185db0f665ed2a265ea60f6e Mon Sep 17 00:00:00 2001 From: Dehann Fourie Date: Sat, 19 Sep 2020 23:28:25 -0400 Subject: [PATCH 4/7] Update README.md --- README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/README.md b/README.md index 6c880dba..e9d8f065 100644 --- a/README.md +++ b/README.md @@ -5,6 +5,7 @@ Note current installation requires the latest Gadfly to overcome a Type Piracy p julia> ] # to activate package manager (@v1.5) pkg> add Gadfly#master (@v1.5) pkg> add IncrementalInference RoME Caesar +``` # RoME.jl From c87c89b761183eba193d91e8c58664162da2a4bd Mon Sep 17 00:00:00 2001 From: "github-actions[bot]" <41898282+github-actions[bot]@users.noreply.github.com> Date: Thu, 8 Oct 2020 00:58:58 +0000 Subject: [PATCH 5/7] CompatHelper: bump compat for "Distributions" to "0.24" --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 42ef4226..1c02195a 100644 --- a/Project.toml +++ b/Project.toml @@ -33,7 +33,7 @@ TransformUtils = "9b8138ad-1b09-5408-aa39-e87ed6d21b63" ApproxManifoldProducts = "0.1.4" CoordinateTransformations = "0.5, 0.6, 0.7" DistributedFactorGraphs = "0.9, 0.10" -Distributions = "0.20, 0.21, 0.22, 0.23" +Distributions = "0.20, 0.21, 0.22, 0.23, 0.24" DocStringExtensions = "0.7, 0.8, 0.9, 0.10, 1" FileIO = "1.0.2, 1.1, 1.2" Graphs = "0.10.2, 0.11, 1" From 7f294171b9246c9bd129a9a5322f36db2958e0bd Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 12 Oct 2020 22:15:08 -0400 Subject: [PATCH 6/7] cleanup --- src/factors/flux/FluxModelsPose2Pose2.jl | 121 +++++++++++++---------- src/factors/flux/models/Pose2OdoNN_01.jl | 12 +-- test/testPoint2Point2Init.jl | 4 - 3 files changed, 77 insertions(+), 60 deletions(-) diff --git a/src/factors/flux/FluxModelsPose2Pose2.jl b/src/factors/flux/FluxModelsPose2Pose2.jl index 832adf9b..8dddec97 100644 --- a/src/factors/flux/FluxModelsPose2Pose2.jl +++ b/src/factors/flux/FluxModelsPose2Pose2.jl @@ -34,10 +34,10 @@ end -function calcVelocityInterPose2!(nfb::FluxModelsPose2Pose2, - iPts::AbstractMatrix{<:Real}, - jPts::AbstractMatrix{<:Real}, - idx::Int ) +function calcVelocityInterPose2!( nfb::FluxModelsPose2Pose2, + iPts::AbstractMatrix{<:Real}, + jPts::AbstractMatrix{<:Real}, + idx::Int ) # # DXY[1:2,i] .= TransformUtils.R(iPts[3,i])'*DXY[1:2,i] nfb.joyVelData[1,3:4] .= jPts[1:2,idx] @@ -54,9 +54,9 @@ function calcVelocityInterPose2!(nfb::FluxModelsPose2Pose2, end -function calcVelocityInterPose2!(nfb::FluxModelsPose2Pose2, - iPts::AbstractMatrix{<:Real}, - jPts::AbstractMatrix{<:Real} ) +function calcVelocityInterPose2!( nfb::FluxModelsPose2Pose2, + iPts::AbstractMatrix{<:Real}, + jPts::AbstractMatrix{<:Real} ) # @assert size(jPts,2) == size(iPts,2) "sampleFluxModelsPose2Pose2 can currently only evaluate equal population size variables" @@ -74,50 +74,20 @@ function calcVelocityInterPose2!(nfb::FluxModelsPose2Pose2, end -# function FMP2P2Other() -# calcVelocityInterPose2!(nfb, getVal(Xi), getVal(Xj) ) -# # sample predictive fraction -# -# # and predict -# # A = [rand(4) for i in 1:25] -# smplsPredAll = nfb.predictFnc(nfb.joyVelData) -# # add rotation dimension if not done by the prediction function -# if size(smplsPredAll,2) == 2 -# smplsPredAll = hcat(smplsPredAll, zeros(size(smplsPredAll,1))) -# end -# -# -# Nn = sum(selectSource .== 2) -# # calculate desired number of predicted values -# Np = N - Nn -# # randomly select particles for prediction (with possible duplicates forwhen Np > size(iPts,2)) -# Npp = Np < numModels ? Np : numModels -# Nnn = N - Npp -# selPreds = @view selectPredModel[1:Npp] # TODO better in-place -# smpls_p = smplsPredAll[selPreds,:] # sample per row?? -# smpls_p[:,3] .= smplsNaive[3,Nnn+1:N] # use naive delta theta at this time -# # naive fraction -# smpls_n = @view smplsNaive[:, 1:Nnn] # sample per column -# # join and shuffle predicted odo values -# shfSmpl = shuffle!(1:N |> collect) -# smpls = hcat(smpls_n, smpls_p')[:,shfSmpl] -# end - - function sampleFluxModelsPose2Pose2(nfb::FluxModelsPose2Pose2, N::Int, fmd::FactorMetadata, Xi::DFGVariable, - Xj::DFGVariable)::Tuple - # + Xj::DFGVariable ) + # # get the naive samples # model samples (all for theta at this time) smplsNaive = rand(nfb.naiveModel, N) - + # calculate naive model and Predictive fraction of samples, respectively selectSource = rand(Categorical([nfb.naiveFrac[]; 1-nfb.naiveFrac[]]), N) - + # number of predictors to choose from, and choose random subset numModels = length(nfb.allPredModels) allPreds = 1:numModels |> collect # 1:Npreds |> collect @@ -129,21 +99,21 @@ function sampleFluxModelsPose2Pose2(nfb::FluxModelsPose2Pose2, # samples for the order in which to use models, dont shuffle if N models # can suppress shuffle for NN training purposes 1 < numModels && nfb.shuffle[] ? shuffle!(allPreds) : nothing - + # cache the time difference estimate nfb.DT[] = (getTimestamp(Xj) - getTimestamp(Xi)).value * 1e-3 - + return (smplsNaive, selectSource, allPreds) end # Convenience function to help call the right constuctor -FluxModelsPose2Pose2(allModels::Vector{P}, - jvd::D, - naiveModel::M, - naiveFrac::Real=0.5, - ss::Function=sampleFluxModelsPose2Pose2, - DT::Real=0.0, - shuffle::Bool=true ) where {P, M <: SamplableBelief, D <: AbstractMatrix} = FluxModelsPose2Pose2{P,D,M}( +FluxModelsPose2Pose2( allModels::Vector{P}, + jvd::D, + naiveModel::M, + naiveFrac::Real=0.5, + ss::Function=sampleFluxModelsPose2Pose2, + DT::Real=0.0, + shuffle::Bool=true ) where {P, M <: SamplableBelief, D <: AbstractMatrix} = FluxModelsPose2Pose2{P,D,M}( allModels, jvd, naiveModel, @@ -176,6 +146,23 @@ function (nfb::FluxModelsPose2Pose2)( nothing end +""" + $SIGNATURES + +Helper function to assemble the data required for each model. +""" +function assembleNNFactorData(dfg::AbstractDFG, fctsym::Symbol, idx::Int) + # + f1 = getFactor(dfg, fctsym) + nfb = getFactorType(f1) + varsyms = getVariableOrder(f1) + Xi = getVariable(dfg, varsyms[1]) |> getBelief |> getPoints + Xj = getVariable(dfg, varsyms[2]) |> getBelief |> getPoints + + # add the specific velocity information + calcVelocityInterPose2!(nfb, Xi, Xj, idx) + nfb.joyVelData +end ## packing converters @@ -273,4 +260,38 @@ end + + +# function FMP2P2Other() +# calcVelocityInterPose2!(nfb, getVal(Xi), getVal(Xj) ) +# # sample predictive fraction +# +# # and predict +# # A = [rand(4) for i in 1:25] +# smplsPredAll = nfb.predictFnc(nfb.joyVelData) +# # add rotation dimension if not done by the prediction function +# if size(smplsPredAll,2) == 2 +# smplsPredAll = hcat(smplsPredAll, zeros(size(smplsPredAll,1))) +# end +# +# +# Nn = sum(selectSource .== 2) +# # calculate desired number of predicted values +# Np = N - Nn +# # randomly select particles for prediction (with possible duplicates forwhen Np > size(iPts,2)) +# Npp = Np < numModels ? Np : numModels +# Nnn = N - Npp +# selPreds = @view selectPredModel[1:Npp] # TODO better in-place +# smpls_p = smplsPredAll[selPreds,:] # sample per row?? +# smpls_p[:,3] .= smplsNaive[3,Nnn+1:N] # use naive delta theta at this time +# # naive fraction +# smpls_n = @view smplsNaive[:, 1:Nnn] # sample per column +# # join and shuffle predicted odo values +# shfSmpl = shuffle!(1:N |> collect) +# smpls = hcat(smpls_n, smpls_p')[:,shfSmpl] +# end + + + + # diff --git a/src/factors/flux/models/Pose2OdoNN_01.jl b/src/factors/flux/models/Pose2OdoNN_01.jl index 72cfad28..85f92239 100644 --- a/src/factors/flux/models/Pose2OdoNN_01.jl +++ b/src/factors/flux/models/Pose2OdoNN_01.jl @@ -4,12 +4,12 @@ export buildPyNNModel_01_FromElements, buildPyNNModel_01_FromWeights, loadTfMode export buildPose2OdoNN_01_FromElements, buildPose2OdoNN_01_FromWeights, loadPose2OdoNNModelIntoFlux -function buildPose2OdoNN_01_FromElements(W1::AbstractMatrix{<:Real}=zeros(4,8), - b1::AbstractVector{<:Real}=zeros(8), - W2::AbstractMatrix{<:Real}=zeros(8,48), - b2::AbstractVector{<:Real}=zeros(8), - W3::AbstractMatrix{<:Real}=zeros(2,8), - b3::AbstractVector{<:Real}=zeros(2)) +function buildPose2OdoNN_01_FromElements( W1::AbstractMatrix{<:Real}=zeros(4,8), + b1::AbstractVector{<:Real}=zeros(8), + W2::AbstractMatrix{<:Real}=zeros(8,48), + b2::AbstractVector{<:Real}=zeros(8), + W3::AbstractMatrix{<:Real}=zeros(2,8), + b3::AbstractVector{<:Real}=zeros(2)) # # W1 = randn(Float32, 4,8) # b1 = randn(Float32,8) diff --git a/test/testPoint2Point2Init.jl b/test/testPoint2Point2Init.jl index 55f6f90e..64431a71 100644 --- a/test/testPoint2Point2Init.jl +++ b/test/testPoint2Point2Init.jl @@ -61,10 +61,6 @@ tree, smt, hist = solveTree!(fg) -# ett = ExploreTreeType(fg, tree, tree.cliques[1], nothing, NBPMessage[]) -# downMsgPassingIterative!(ett,N=100, dbg=false, drawpdf=false); - - @test 10 < sum( 80 .< getVal(fg, :l1)[1,:] .< 120 ) @test 10 < sum( -20 .< getVal(fg, :l1)[2,:] .< 20 ) From 5c6660da7a0618480e8e85b278930d816041404e Mon Sep 17 00:00:00 2001 From: dehann Date: Mon, 12 Oct 2020 22:15:57 -0400 Subject: [PATCH 7/7] bump v0.9.2 --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 42ef4226..3a8e5413 100644 --- a/Project.toml +++ b/Project.toml @@ -2,7 +2,7 @@ name = "RoME" uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8" keywords = ["SLAM", "state-estimation", "MM-iSAM", "inference", "robotics"] desc = "Non-Gaussian simultaneous localization and mapping" -version = "0.9.1" +version = "0.9.2" [deps] ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"