Merge branch 'master' into twig/4Q20/v0.9.2

JuliaRobotics · Oct 13, 2020 · 1e6bcd6 · 1e6bcd6
2 parents b55d2d6 + a1ab823
commit 1e6bcd6
Show file tree

Hide file tree

Showing 5 changed files with 89 additions and 63 deletions.
diff --git 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"
@@ -33,11 +33,11 @@ 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"
-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"

diff --git a/README.md b/README.md
@@ -1,3 +1,12 @@
+# 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 |

diff --git 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
@@ -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
@@ -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 )