Merge pull request #879 from JuliaRobotics/22Q3/enh/SAP3

better standz. ScatterAlign as PosePose residual (Pose3)

Author: dehann

Project.toml

@@ -2,7 +2,7 @@ name = "Caesar"
 uuid = "62eebf14-49bc-5f46-9df9-f7b7ef379406"
 keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics", "ROS"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.13.1"
+version = "0.13.2"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"
@@ -44,35 +44,35 @@ Unmarshal = "cbff2730-442d-58d7-89d1-8e530c41eb02"
 YAML = "ddb6d928-2868-570f-bddf-ab3f9cf99eb6"
 
 [compat]
-ApproxManifoldProducts = "0.4.21, 0.5, 0.6"
+ApproxManifoldProducts = "0.6"
 AprilTags = "0.8, 0.9"
-Combinatorics = "0.7, 0.8, 0.9, 1"
+Combinatorics = "1"
 CoordinateTransformations = "0.5, 0.6"
-DataStructures = "0.16, 0.17, 0.18"
-DistributedFactorGraphs = "0.17, 0.18"
-Distributions = "0.22, 0.23, 0.24, 0.25"
-DocStringExtensions = "0.7, 0.8, 0.9"
+DataStructures = "0.17, 0.18"
+DistributedFactorGraphs = "0.18"
+Distributions = "0.25"
+DocStringExtensions = "0.8, 0.9"
 FFTW = "1"
 FileIO = "1"
-ImageCore = "0.7, 0.8, 0.9"
-ImageMagick = "0.7, 1.0, 1.1"
-IncrementalInference = "0.26, 0.27, 0.28, 0.29, 0.30"
+ImageCore = "0.8, 0.9"
+ImageMagick = "1"
+IncrementalInference = "0.30"
 JLD2 = "0.3, 0.4"
-JSON = "0.19, 0.20, 0.21"
+JSON = "0.20, 0.21"
 JSON2 = "0.3, 0.4"
 KernelDensityEstimate = "0.5"
-Manifolds = "0.6.3, 0.7, 0.8"
-NLsolve = "3, 4"
+Manifolds = "0.8"
+NLsolve = "4"
 Optim = "1"
-ProgressMeter = "0.9, 1"
-Reexport = "0.2, 1"
-Requires = "0.5, 1"
-RoME = "0.17, 0.18, 0.19, 0.20"
+ProgressMeter = "1"
+Reexport = "1"
+Requires = "1"
+RoME = "0.20"
 Rotations = "1.1"
 StaticArrays = "1"
-TensorCast = "0.3, 0.4"
+TensorCast = "0.4"
 TimeZones = "1.3.1, 1.4"
-TransformUtils = "0.2.2"
+TransformUtils = "0.2.14"
 Unmarshal = "0.3, 0.4"
 YAML = "0.3, 0.4"
 julia = "1.6"

src/images/ScatterAlignPose2.jl

@@ -64,6 +64,7 @@ struct ScatterAlignPose3 <: IIF.AbstractManifoldMinimize
   align::ScatterAlign{Pose3,<:Any,<:Any}
 end
 
+_getPoseType(::ScatterAlign{P}) where P = P
 
 # replace inner constructor with transform on image
 function ScatterAlignPose2(im1::AbstractMatrix{T}, 
@@ -156,9 +157,8 @@ getManifold(::IIF.InstanceType{<:ScatterAlignPose3}) = getManifold(Pose3Pose3)
 # runs once upon addFactor! and returns object later used as `cache`
 function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, fnc::Union{<:ScatterAlignPose2,<:ScatterAlignPose3})
   #
-  _getPoseType(sa::ScatterAlign{P}) where P = P
   M = getManifold(_getPoseType(fnc.align))
-  e0 = getPointIdentity(M) # ArrayPartition(SVector(0.0,0.0), SMatrix{2,2}(1.0, 0.0, 0.0, 1.0))
+  e0 = getPointIdentity(M)
 
   # reconstitute cloud belief from dataEntry
   for (va,de,cl) in zip(vars,[fnc.align.dataEntry_cloud1,fnc.align.dataEntry_cloud2],[fnc.align.cloud1,fnc.align.cloud2])
@@ -182,10 +182,12 @@ function preambleCache(dfg::AbstractDFG, vars::AbstractVector{<:DFGVariable}, fn
 end
 
 
-function getSample( cf::CalcFactor{<:ScatterAlignPose2} )
+function getSample( cf::CalcFactor{S} ) where {S <: Union{<:ScatterAlignPose2,<:ScatterAlignPose3}}
   #
   M = cf.cache.M
   e0 = cf.cache.e0
+  ntr = length(Manifolds.submanifold_component(e0,1))
+  nrt = Manifolds.manifold_dimension(M)-ntr
   # R0 = submanifold_component(e0,2)
 
   pVi = cf.cache.smps1
@@ -203,7 +205,7 @@ function getSample( cf::CalcFactor{<:ScatterAlignPose2} )
   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(2); 0.1*randn()], Optim.BFGS() )
+  res = Optim.optimize(cost, [5*randn(ntr); 0.1*randn(nrt)], Optim.BFGS() )
 
   cf.cache.score[] = res.minimum
 
@@ -213,20 +215,29 @@ function getSample( cf::CalcFactor{<:ScatterAlignPose2} )
 end
 
 
-function (cf::CalcFactor{<:ScatterAlignPose2})(pXq, wPp, wPq)
+function (cf::CalcFactor{S})(X, p, q
+  ) where {S <: Union{<:ScatterAlignPose2,<:ScatterAlignPose3}}
   # 
 
   M = cf.cache.M
-  e0 = cf.cache.e0
-  
-  # get the current relative transform estimate
-  wPq_ = Manifolds.compose(M, wPp, exp(M, e0, pXq))
-  
-  #TODO allocalte for vee! see Manifolds #412, fix for AD
-  Xc = zeros(3)
-  vee!(M, Xc, wPq, log(M, wPq, wPq_))
+  ϵ0 = cf.cache.e0
 
+  # copied from Pose2Pose2
+  q̂ = allocate(q)
+  exp!(M, q̂, ϵ0, X)
+  Manifolds.compose!(M, q̂, p, q̂)   
+  Xc = vee(M, q, log!(M, q̂, q, q̂))
   return Xc
+
+    # pXq, 
+    # wPp, 
+    # wPq
+  # # get the current relative transform estimate
+  # wPq_ = Manifolds.compose(M, wPp, exp(M, e0, pXq))
+  # #TODO allocalte for vee! see Manifolds #412, fix for AD
+  # Xc = zeros(3)
+  # vee!(M, Xc, wPq, log(M, wPq, wPq_))
+  # return Xc
 end
 
 

test/testScatterAlignPose2.jl

@@ -18,8 +18,8 @@ println("Starting ScatterAlignPose2 tests...")
 @testset "Test ScatterAlignPose2" begin
 ##
 
-x = -10:0.1:10;
-y = -10:0.1:10;
+x = -15:0.1:15;
+y = -15:0.1:15;
 
 σ = 0.1
 
@@ -29,7 +29,7 @@ g = (x,y)->pdf(MvNormal([3.;0],Σ),[x;y]) + pdf(MvNormal([8.;0.0],4*Σ),[x;y]) +
 bIM1 = zeros(length(x),length(y))
 bIM2 = zeros(length(x),length(y))
 
-oT = [2.; 0]
+oT = [5.; 0]
 oΨ =  pi/8
 
 M = SpecialEuclidean(2)
@@ -162,9 +162,6 @@ end
 
 
 
-
-
-
 @testset "test ScatterAlignPose2 with MKD direct" begin
 ##
 
@@ -265,13 +262,13 @@ end
 
 
 
-@testset "test ScatterAlignPose2 with MKD direct" begin
+@testset "test ScatterAlignPose3 with MKD direct" begin
 ##
 
 # setup
 
-oT = [2.; 0;0]
-oΨ =  [0;0;pi/6]
+oT = [2.; 0;2]
+oΨ =  [0;0;pi/10]
 
 M = SpecialEuclidean(3)
 e0 = getPointIdentity(M)
@@ -300,8 +297,25 @@ P2 = manikde!(Point3, p2)
 
 sap = ScatterAlignPose3(;cloud1=P1, cloud2=P2, sample_count=100, bw=1.0)
 
+## sample from ScatterAlignPose3
+
+fg = initfg()
+getSolverParams(fg).inflateCycles=1
+
+addVariable!(fg, :x0, Pose3)
+addVariable!(fg, :x1, Pose3)
+
+addFactor!(fg, [:x0], PriorPose3( MvNormal(Diagonal(map(abs2,0.1*ones(6)))) ))
+addFactor!(fg, [:x0;:x1], sap, inflation=0.0)
+
 ##
 
+Xsmpl = sampleFactor(fg, :x0x1f1)
+
+@test Xsmpl[1] isa ArrayPartition
+@test length(Xsmpl[1]) === 12
+
+##
 end