Merge pull request #927 from JuliaRobotics/22Q4/enh/scatteralignwicp

option for ScatterAlign to use ICP or MMD

Project.toml

@@ -103,4 +103,4 @@ Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 ZMQ = "c2297ded-f4af-51ae-bb23-16f91089e4e1"
 
 [targets]
-test = ["AprilTags", "BSON", "DelimitedFiles", "Downloads", "FixedPointNumbers", "Gadfly", "GraphPlot", "Images", "PyCall", "Random", "RobotOS", "Serialization", "Test", "ZMQ"]
+test = ["AprilTags", "BSON", "DelimitedFiles", "Downloads", "FixedPointNumbers", "Gadfly", "GraphPlot", "Images", "PyCall", "Random", "RobotOS", "Test", "ZMQ"]

src/3rdParty/_PCL/services/ConsolidateRigidTransform.jl

@@ -8,7 +8,7 @@
 # Works for transform of both 2D and 3D  point clouds
 # FIXME, to optimize, this function will likely be slow
 # TODO, consolidate with transformPointcloud(::ScatterAlign,..) function
-function apply( M_::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
+function Manifolds.apply( M_::Union{<:typeof(SpecialEuclidean(2)),<:typeof(SpecialEuclidean(3))},
                 rPp::Manifolds.ArrayPartition,
                 pc::PointCloud{T} ) where T
   #

src/images/ScatterAlignPose2.jl

@@ -17,12 +17,16 @@ export overlayScatter, overlayScatterMutate
     ScatterAlign{P,H1,H2} where  {H1 <: Union{<:ManifoldKernelDensity, <:HeatmapGridDensity}, 
                                   H2 <: Union{<:ManifoldKernelDensity, <:HeatmapGridDensity}}
 
-Alignment factor between point cloud populations, using a continuous density function cost: [`ApproxManifoldProducts.mmd`](@ref).
-Supports very large density clouds, with `sample_count` subsampling for individual alignments.
+Alignment factor between point cloud populations, using either
+- a continuous density function cost: [`ApproxManifoldProducts.mmd`](@ref), or
+- a conventional iterative closest point (ICP) algorithm (when `.sample_count < 0`).
+
+This factor can support very large density clouds, with `sample_count` subsampling for individual alignments.
 
 Keyword Options:
 ----------------
-- `sample_count::Int = 100`, number of subsamples to use during each alignment in `getSample`.
+- `sample_count::Int = 100`, number of subsamples to use during each alignment in `getSample`.  
+  - Values greater than 0 use MMD alignment, while values less than 0 use ICP alignment.
 - `bw::Real`, the bandwidth to use for [`mmd`](@ref) distance
 - `rescale::Real`
 - `N::Int`
@@ -73,7 +77,7 @@ Base.@kwdef struct ScatterAlign{P,
   bw::Float64        = 1.0
   """ EXPERIMENTAL, flag whether to use 'stashing' for large point cloud, see [Stash & Cache](@ref section_stash_unstash) """
   useStashing::Bool = false
-  """ DataEntry ID for hollow store of cloud 1 & 2 """
+  """ DataEntry ID for hollow store of cloud 1 & 2, TODO change to UUID instead """
   dataEntry_cloud1::String = ""
   dataEntry_cloud2::String = ""
   """ Data store hint where likely to find the data entries and blobs for reconstructing cloud1 and cloud2"""
@@ -193,6 +197,24 @@ function ScatterAlignPose3(;
   ScatterAlignPose3(sa)
 end
 
+function ScatterAlignPose3(
+  ::Type{<:ManifoldKernelDensity};
+  cloud1::_PCL.PointCloud,
+  cloud2::_PCL.PointCloud,
+  bw1 = [1;1;1.0],
+  bw2 = bw1,
+  kw...
+)
+  p1 = (s->s.data[1:3]).(cloud1.points) 
+  p2 = (s->s.data[1:3]).(cloud2.points) 
+
+  Mt = TranslationGroup(3)
+  b1 = manikde!(Mt, p1; bw=bw1)
+  b2 = manikde!(Mt, p2; bw=bw2)
+
+  ScatterAlignPose3(;cloud1=b1, cloud2=b2, kw...)
+end
+
 getManifold(::IIF.InstanceType{<:ScatterAlignPose2}) = getManifold(Pose2Pose2)
 getManifold(::IIF.InstanceType{<:ScatterAlignPose3}) = getManifold(Pose3Pose3)
 
@@ -221,8 +243,13 @@ function preambleCache(
     end
   end
 
-  smps1, = sample(fnc.align.cloud1, fnc.align.sample_count)
-  smps2, = sample(fnc.align.cloud2, fnc.align.sample_count)
+  smps1,smps2 = if 0 <= fnc.align.sample_count
+    smps1, = sample(fnc.align.cloud1, fnc.align.sample_count)
+    smps2, = sample(fnc.align.cloud2, fnc.align.sample_count)
+    smps1, smps2
+  else
+    nothing, nothing
+  end
 
   bw = SA[fnc.align.bw;]
 
@@ -242,26 +269,54 @@ function getSample( cf::CalcFactor{S} ) where {S <: Union{<:ScatterAlignPose2,<:
 
   pVi = cf.cache.smps1
   qVj = cf.cache.smps2
-  # Fresh samples
-  for i in 1:cf.factor.align.sample_count
-    pVi[i] .= sample(cf.factor.align.cloud1)[1][1]
-    qVj[i] .= sample(cf.factor.align.cloud2)[1][1]
-  end
 
-  # qVi(qCp) = _transformPointCloud(M,pVi,qCp)
+  # TODO consolidate this transform with methods used by ICP
   pVj(qCp) = _transformPointCloud(M,qVj,qCp; backward=true)
 
-  # bw = SA[cf.factor.bw;]
-  cost(xyr) = mmd(M.manifold[1], pVi, pVj(xyr), length(pVi), length(qVj), cf._allowThreads; cf.cache.bw)
-
-  # return mmd as residual for minimization
-  res = Optim.optimize(cost, [5*randn(ntr); 0.1*randn(nrt)], Optim.BFGS() )
-
-  cf.cache.score[] = res.minimum
-
-  # give measurement relative to e0 identity
-  #  TODO relax to Riemannian where e0 is replaced by any point
-  hat(M, e0, res.minimizer)
+  if 0 <= cf.factor.align.sample_count
+    # Fresh samples
+    for i in 1:cf.factor.align.sample_count
+      pVi[i] .= sample(cf.factor.align.cloud1)[1][1]
+      qVj[i] .= sample(cf.factor.align.cloud2)[1][1]
+    end
+
+    # qVi(qCp) = _transformPointCloud(M,pVi,qCp)
+    # pVj(qCp) = _transformPointCloud(M,qVj,qCp; backward=true)
+    # bw = SA[cf.factor.bw;]
+    cost(xyr) = mmd(M.manifold[1], pVi, pVj(xyr), length(pVi), length(qVj), cf._allowThreads; cf.cache.bw)
+    # return mmd as residual for minimization
+    # FIXME upgrade to the on manifold optimization
+    res = Optim.optimize(cost, [5*randn(ntr); 0.1*randn(nrt)], Optim.BFGS() )
+    cf.cache.score[] = res.minimum
+    # give measurement relative to e0 identity
+    #  TODO relax to Riemannian where e0 is replaced by any point
+    return hat(M, e0, res.minimizer)
+  else #if cf.factor.align.sample_count < 0
+    @assert cf.factor.align.cloud1 isa ManifoldKernelDensity "ICP alignments currently only implemented for beliefs as MKDs"
+    ppt = getPoints(cf.factor.align.cloud1)
+    qpt_ = getPoints(cf.factor.align.cloud2)
+
+    # bump pc before alignment to get diversity in sample population
+    dstb = 0.2*randn(getDimension(M)) # retract(M,e0,hat(M,e0,0.2*randn(getDimension(M))))
+    qpt = _transformPointCloud(M, qpt_, dstb; backward=true)
+
+    # FIXME, super excessive repeat of data wrangling in hot loop
+    @cast p_ptsM[i,d] := ppt[i][d]
+    @cast phat_pts_mov[i,d] := qpt[i][d]
+
+    # do actual alignment with ICP
+    p_Hicp_phat, Hpts_mov, status = _PCL.alignICP_Simple(
+      p_ptsM, 
+      phat_pts_mov; 
+      verbose=false,
+      max_iterations = 25,
+      correspondences = 500,
+      neighbors = 50
+    )
+    # convert SE affine H to tangent vector X for use in residual function 
+    p_P_q = ArrayPartition(p_Hicp_phat[1:end-1,end],p_Hicp_phat[1:end-1,1:end-1])
+    return log(M, e0, p_P_q)
+  end
 end
 
 

test/runtests.jl

@@ -15,6 +15,7 @@ if TEST_GROUP in ["all", "basic_functional_group"]
     # highly multipackage tests that don't fit well in specific library dependencies.
     include("testScatterAlignParched.jl")
     include("testScatterAlignPose2.jl")
+    incoude("testScatterAlignPose3.jl")
     include("testStashing_SAP.jl")
     include("pcl/testPointCloud2.jl")
     include("testPose2AprilTag4Corner.jl")

test/testPose2AprilTag4Corner.jl

@@ -164,8 +164,8 @@ fct = getFactorType(fg, :x0tag17f1)
 
 preImgs = zeros(5,10)
 
-
-for i in 1:10
+# preformance regression, see https://github.com/JuliaRobotics/Caesar.jl/issues/928
+for i in 1:3
   println("finding preimage $i") 
   preImgs[:,i] .= _solveFactorPreimage(fct, pred[i], regularize=0.001)
 

test/testScatterAlignPose3.jl

@@ -0,0 +1,91 @@
+
+using Test
+using Caesar
+# import Caesar._PCL as _PCL
+# using Downloads
+# using DelimitedFiles
+# using TensorCast
+using Images
+# using JSON
+
+const _PCL = Caesar._PCL
+
+##
+
+@testset "test ScatterAlignPose3 on ICP" begin
+##
+
+## ~50mb download, blobs that are too big to always store in variables themselves
+# lidar1_url = "https://github.com/JuliaRobotics/CaesarTestData.jl/raw/main/data/lidar/simpleICP/terrestrial_lidar1.xyz"
+# lidar2_url = "https://github.com/JuliaRobotics/CaesarTestData.jl/raw/main/data/lidar/simpleICP/terrestrial_lidar2.xyz"
+# io1 = PipeBuffer()
+# io2 = PipeBuffer()
+# Downloads.download(lidar1_url, io1)
+# Downloads.download(lidar2_url, io2)
+
+# X_fix = readdlm(io1)
+# X_mov = readdlm(io2)
+
+X_fix = randn(10000,3)
+X_mov = randn(10000,3)
+
+pcf = _PCL.PointCloud(X_fix)
+pcm = _PCL.PointCloud(X_mov)
+
+# @cast pts_f_[i][d] := X_fix[i,d]
+# @cast pts_m_[i][d] := X_mov[i,d]
+
+# pts_f = (s->[s...]).(pts_f_)
+# pts_m = (s->[s...]).(pts_m_)
+
+
+##
+
+fg = initfg()
+
+addVariable!(fg, :x0, Pose3)
+addVariable!(fg, :x1, Pose3)
+
+##
+
+# this is how how you call the ICP version of SAP
+sap = Caesar.ScatterAlignPose3(
+  ManifoldKernelDensity;
+  cloud1=pcf, 
+  cloud2=pcm,
+  sample_count=-1,
+  useStashing=false,
+)
+
+## this line checks blob store access via preambleCache specifically for ScatterAlign which will internally access the blob store
+
+f1 = addFactor!(fg, [:x0; :x1], sap; graphinit=false);
+
+## make sure stuff is working before serialization
+meas = sampleFactor(fg, getLabel(f1))
+
+
+@warn "Testing of ICP based ScatterAlignPose3 serialization must still be done"
+## serialize with stashing enabled, see docs here: https://juliarobotics.org/Caesar.jl/latest/concepts/stash_and_cache/
+
+# pf = DFG.packFactor(fg, getFactor(fg, getLabel(f1)))
+# jpf = JSON.json(pf)
+
+# check that the massive point clouds are not stored in the packed factor object
+# @test length(jpf) < 1100
+
+## now confirm the solver deserialization can also work with the factor pulling data from the blob store 
+
+# tfg = initfg()
+
+# # use existing logpath from fg
+# storeDir = "/tmp/caesar/localstore" # joinLogPath(fg,"data")
+
+# datastore = FolderStore{Vector{UInt8}}(:default_folder_store, storeDir) 
+# addBlobStore!(tfg, datastore)
+# v0 = addVariable!(tfg, :x0, Pose3)
+# v1 = addVariable!(tfg, :x1, Pose3)
+
+
+##
+end