Merge pull request #1445 from JuliaRobotics/master

v0.25.6-rc1

Project.toml

@@ -2,7 +2,7 @@ name = "IncrementalInference"
 uuid = "904591bb-b899-562f-9e6f-b8df64c7d480"
 keywords = ["MM-iSAMv2", "Bayes tree", "junction tree", "Bayes network", "variable elimination", "graphical models", "SLAM", "inference", "sum-product", "belief-propagation"]
 desc = "Implements the Multimodal-iSAMv2 algorithm."
-version = "0.25.5"
+version = "0.25.6"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

src/services/HeatmapSampler.jl

@@ -15,6 +15,8 @@ export sampleHeatmap
 getManifold(hgd::HeatmapGridDensity) = getManifold(hgd.densityFnc)
 getManifold(lsg::LevelSetGridNormal) = getManifold(lsg.heatmap)
 
+AMP.sample(hgd::HeatmapGridDensity,w...;kw...) = sample(hgd.densityFnc, w...;kw...)
+
 """
     $SIGNATURES
 
@@ -104,6 +106,19 @@ function HeatmapGridDensity(data::AbstractMatrix{<:Real},
   HeatmapGridDensity(data, domain, hint_callback, bw_factor, density)
 end
 
+function Base.isapprox( a::HeatmapGridDensity, b::HeatmapGridDensity; 
+                        atol::Real=1e-10, mmd_tol::Real=1e-2)
+  #
+  isapprox( Npts(a.densityFnc), Npts(b.densityFnc) ; atol) ? nothing : (return false)
+  isapprox( a.densityFnc, b.densityFnc; atol=mmd_tol) ?      nothing : (return false)
+  isapprox( a.data, b.data ; atol) ?                         nothing : (return false)
+  isapprox( a.domain[1], b.domain[1] ; atol) ?               nothing : (return false)
+  isapprox( a.domain[2], b.domain[2] ; atol) ?               nothing : (return false)
+
+  return true
+end
+
+
 
 # legacy construct helper
 function LevelSetGridNormal(data::AbstractMatrix{<:Real}, 
@@ -115,7 +130,7 @@ function LevelSetGridNormal(data::AbstractMatrix{<:Real},
                             bw_factor::Real=0.7,  # kde spread between domain points 
                             N::Int=10000  )
   #
-
+  
   # select the support from raw data
   roi = data.-level
   # make Gaussian
@@ -129,7 +144,7 @@ function LevelSetGridNormal(data::AbstractMatrix{<:Real},
   #   masked_roi = 0 .< κ^2 - l
 
   hgd = HeatmapGridDensity(data, domain, hint_callback, bw_factor; N=N)
-
+  
   LevelSetGridNormal(level, sigma, float(sigma_scale), hgd)
 end
 

test/runtests.jl

@@ -10,7 +10,7 @@ include("testPartialNH.jl")
 include("testSphereMani.jl")
 include("testSpecialOrthogonalMani.jl")
 
-
+include("testHeatmapGridDensity.jl")
 include("testCliqSolveDbgUtils.jl")
 include("TestModuleFunctions.jl")
 include("testApproxConv.jl")

test/testHeatmapGridDensity.jl

@@ -0,0 +1,38 @@
+# test ScatterAlignPose2
+
+using Test
+using IncrementalInference
+using Distributions
+
+
+##
+@testset "Test HeatmapGridDensity" begin
+##
+
+
+x = -10:0.1:10;
+y = -10:0.1:10;
+
+img = zeros(length(x),length(y))
+
+g = (x,y)->pdf(MvNormal([1.0;1.0]),[x;y])
+
+for (i,x_) in enumerate(x), (j,y_) in enumerate(y)
+  img[i,j] = g(x_,y_)
+end
+
+hgd = IIF.HeatmapGridDensity(img,(x,y),nothing, 0.07, N=1000)
+
+# check conversions to packed types
+phgd = convert(PackedSamplableBelief, hgd)
+hgd_ = convert(SamplableBelief, phgd)
+
+@test isapprox( hgd, hgd_ )
+
+# use in graph
+
+
+
+
+##
+end