Merge pull request #691 from JuliaRobotics/23Q3/enh/testgenprojcam

refac solveMultiviewLandmark! and test

ext/RoMECameraModelsExt.jl

@@ -1,22 +1,154 @@
 module RoMECameraModelsExt
 
-@info "RoME.jl is loading extension functionality using CameraModels.jl"
+# @info "RoME.jl is loading extension functionality using CameraModels.jl"
 
 using CameraModels
+using StaticArrays
+using Manifolds
+using DocStringExtensions
+using Optim
 
-import IncrementalInference: CalcFactor
-import RoME: GenericProjection
+import IncrementalInference: AbstractDFG, getFactorType, getVariable, getSolverData, CalcFactor, ls
+import RoME: GenericProjection, solveMultiviewLandmark!
 
 
-function (CalcFactor{<:GenericProjection{S,T}})(
+function projectPointFrom(cam, c_H_w, w_Ph)
+  c_Ph = c_H_w*w_Ph |> SVector{4}
+  CameraModels.projectHomogeneous(cam,c_Ph)
+end
+
+function (cf::CalcFactor{<:GenericProjection{S,T}})(
   c_X,
   w_P_c,
   w_P_o
 ) where {S,T}
+
+  κ = 1000
+  M = SpecialEuclidean(3)
+
+  w_Ph = SVector{4,Float64}(w_P_o..., 1.0)
+  c_H_w = inv(affine_matrix(M, w_P_c))
+
   # predicted projection
-  c_Xhat = project(S, T, w_P_c, w_P_o)
+  # c_Xhat = project(S, T, w_P_c, w_P_o)
+  pred = projectPointFrom(cf.factor.cam, c_H_w, w_Ph)
+  # experimental cost function to try force bad reprojects in front of the camera during optimization
+  # κ*(abs(pred.depth) - pred.depth)^2 + (c_X[1]-pred[1])^2 + (c_X[2]-pred[2])^2
+
+  res = SVector{3,Float64}(
+    c_X[1]-pred[1],
+    c_X[2]-pred[2],
+    κ*(abs(pred.depth) - pred.depth)
+  )
+
   # error vs measured projection
-  return c_X - c_Xhat
+  # return c_X - c_Xhat
+  return res
 end
 
+
+
+
+
+
+
+"""
+    $SIGNATURES
+
+native Optim solution to complement GenericProjection factor development.
+
+Notes
+- See RoME/test/testGenericProjection.jl for usage example.
+- Original implementation from JuliaRobotics/CameraModels.jl/test/multiview_manifolds.jl
+"""
+function solveMultiviewLandmark!(
+  dfg::AbstractDFG, 
+  lmlb::Symbol;
+  cam = CameraModels.CameraCalibration(),
+  retry::Int = 100
+)
+  # assume to find 
+  M = SpecialEuclidean(3)
+  flbs = ls(dfg, lmlb)
+  # fcs = getFactor.(dfg, flbs)
+  # fcd = getFactorType.(fcs)
+
+  function projectPointFrom(cam, c_H_w, w_Ph)
+    c_Ph = c_H_w*w_Ph |> SVector{4}
+    CameraModels.projectHomogeneous(cam,c_Ph)
+  end
+
+  function cameraResidual(cam, meas, M, w_T_c, w_Ph, κ=1000)
+    pred = projectPointFrom(cam, inv(affine_matrix(M,w_T_c)), w_Ph)
+    # experimental cost function to try force bad reprojects in front of the camera during optimization
+    κ*(abs(pred.depth) - pred.depth)^2 + (meas[1]-pred[1])^2 + (meas[2]-pred[2])^2
+  end
+
+  function cost(cam, c_Xi, w_P_ci, w_Ph)
+    res = 0.0
+    for (c_X, w_P_c) in zip(c_Xi, w_P_ci)
+      res += cameraResidual(cam, c_X, M, w_P_c, w_Ph)
+    end
+    return res
+  end
+
+  _w_P_ci = Vector{ArrayPartition{Float64, Tuple{SVector{3, Float64}, SMatrix{3, 3, Float64, 9}}}}()
+  vlbs = Symbol[]
+  _c_Xi = Vector{CameraModels.PixelIndex{true, Float64}}()
+  for fl in flbs
+    vl = setdiff(ls(dfg, fl), [lmlb;])[1]
+    push!(
+      _w_P_ci,
+      getSolverData(getVariable(dfg, vl), :parametric).val[1]
+    )
+    union!(
+      vlbs,
+      [vl;],
+    )
+
+    obs = CameraModels.PixelIndex(getFactorType(dfg, fl).Z.μ...)
+    push!(_c_Xi, obs)
+  end
+  cost_(w_Lh) = cost(cam, _c_Xi, _w_P_ci, SVector(w_Lh...))
+
+  w_P3 = zeros(3)
+  for i in 1:retry
+    w_Ph0 = [(retry*randn(3) .+ getSolverData(getVariable(dfg, lmlb), :parametric).val[1])...; 1.0]
+    # w_Ph0 = [10;0;0;1.0]
+    w_Res = Optim.optimize(
+      cost_, 
+      w_Ph0, # [1;0;0;1.], 
+      LBFGS(),
+      # ParticleSwarm(; lower = [0.,-1.,-1.,0.],
+      #               upper = [99999.;1;1;9999],
+      #               n_particles = 10),
+      # Optim.Options(g_tol = 1e-12,
+      #               iterations = 100,
+      #               store_trace = false,
+      #               show_trace = true);
+      # autodiff=:forward
+    )
+    w_P3 = w_Res.minimizer |> CameraModels.toNonhomogeneous
+    # check depth okay
+    w_Ph = [w_P3...; 1.0]
+    depthok = true
+    for w_T_c in _w_P_ci 
+      pred = projectPointFrom(cam, inv(affine_matrix(M, w_T_c)), w_Ph)
+      if pred.depth < 0
+        depthok = false
+      end
+    end
+    if w_Res.ls_success && depthok
+      break
+    end
+    if retry <= i 
+      throw(DomainError("Unable to converge projection solution"))
+    end
+  end
+
+  return w_P3
+end
+
+
+
 end

ext/WeakdepsPrototypes.jl

@@ -2,5 +2,7 @@
 # weakdeps functions for ImageIOExt
 function generateField_CanyonDEM end
 
+function solveMultiviewLandmark! end
+
 # Flux.jl
 function FluxModelsPose2Pose2 end

ext/factors/GenericProjection.jl

@@ -21,9 +21,13 @@ gp = GenericProjection{
 )
 ```
 """
-@kwdef struct GenericProjection{SRC<:InferenceVariable,TRG<:InferenceVariable,C,D}
+@kwdef struct GenericProjection{SRC<:InferenceVariable,TRG<:InferenceVariable,C,D} <: AbstractManifoldMinimize
+  cam::C
   Z::D
-  calib::C
 end
 
+GenericProjection{SRC,TRG}(cam::C, Z::D) where {SRC<:InferenceVariable,TRG<:InferenceVariable,C,D} = GenericProjection{SRC,TRG,C,D}(;cam,Z)
+
+
 getManifold(gp::GenericProjection{S,T}) where {S, T} = TranslationGroup(getDimension(gp.Z))
+

src/ExportAPI.jl

@@ -241,7 +241,7 @@ export homography_to_coordinates, coordinates_to_homography
 export generateField_CanyonDEM
 
 # CameraModels
-export GenericProjection
+export GenericProjection, solveMultiviewLandmark!
 
 # Flux.jl
 export MixtureFluxPose2Pose2, PackedMixtureFluxPose2Pose2

test/runtests.jl

@@ -51,6 +51,7 @@ testfiles = [
   "testTreeInitCommonMsg_IIF913.jl";
   "testHexagonal2D_CliqByCliq.jl";      # special case debugging
   "testhigherdimroots.jl";
+  "testGenericProjection.jl";
   "testDidsonFunctions.jl";
   "testBasicPose2Stationary.jl";
   "TestPoseAndPoint2Constraints.jl";

test/testGenericProjection.jl

@@ -0,0 +1,115 @@
+
+
+# using Revise
+using Test
+using CameraModels
+using RoME
+using Manifolds
+
+# using ManifoldDiff
+# import FiniteDifferences as FD
+
+
+##
+@testset "GenericProjection of 2 camera poses to a point" begin
+##
+
+cam = CameraModels.CameraCalibration()
+
+obs1 = CameraModels.PixelIndex(240, 320)
+obs2 = CameraModels.PixelIndex(240, 315)
+
+w_T_c1 = ArrayPartition([0; 0  ;0.],[0 0 1; -1 0 0; 0 -1 0.])
+w_T_c2 = ArrayPartition([0;-0.1;0.],[0 0 1; -1 0 0; 0 -1 0.])
+
+# w
+# [
+# 0 -0.1 0
+# ]
+# =
+# w
+# [
+#  0  0  1
+# -1  0  0
+#  0 -1  0
+# ]
+# c
+# [
+# 0.1 0 0
+# ]
+
+##
+
+fg = initfg()
+getSolverParams(fg).graphinit = false
+
+addVariable!(fg, :w_P_c1, Pose3)
+addVariable!(fg, :w_P_c2, Pose3)
+addVariable!(fg, :w_Ph, Point3)
+
+c1 = manikde!(Pose3, [w_T_c1 for _ in 1:1], bw=ones(6));
+c2 = manikde!(Pose3, [w_T_c2 for _ in 1:1], bw=ones(6));
+h1 = manikde!(Position3, [zeros(3) for _ in 1:1], bw=ones(3));
+
+initVariable!(fg, :w_P_c1, c1, :parametric)
+initVariable!(fg, :w_P_c2, c2, :parametric)
+initVariable!(fg, :w_Ph, h1, :parametric)
+
+Z=MvNormal([240.0;320], [1 0; 0 1.])
+f1 = RoME.GenericProjection{Pose3,Point3}(cam, Z)
+addFactor!(fg, [:w_P_c1; :w_Ph], f1)
+
+Z=MvNormal([240.0;315], [1 0; 0 1.])
+f2 = RoME.GenericProjection{Pose3,Point3}(cam, Z)
+addFactor!(fg, [:w_P_c2; :w_Ph], f2)
+
+# FIXME, should be done on parametric initVariable above
+getSolverData(fg[:w_P_c1], :parametric).bw = diagm(ones(6))
+getSolverData(fg[:w_P_c2], :parametric).bw = diagm(ones(6))
+getSolverData(fg[:w_Ph], :parametric).bw = diagm(ones(3))
+
+M = getManifold(fg, :w_P_c1)
+addFactor!(
+  fg, 
+  [:w_P_c1,], 
+  PriorPose3(
+    MvNormal(
+      vee(M, identity_element(M), log(M,identity_element(M),w_T_c1)),
+      diagm(0.01*ones(6))
+    )
+  )
+)
+
+addFactor!(
+  fg, 
+  [:w_P_c2,], 
+  PriorPose3(
+    MvNormal(
+      vee(M, identity_element(M), log(M,identity_element(M),w_T_c2)),
+      diagm(0.01*ones(6))
+    )
+  )
+)
+
+@error "TODO Work in progress for solving GenericProjection factors via solveGraphParametric!"
+# IIF.solveGraphParametric!(fg)
+
+
+##
+
+# using test development function
+w_P3 = solveMultiviewLandmark!(fg, :w_Ph)
+
+
+##
+
+
+@test isapprox([10.56;0;0], w_P3; atol=1e-3)
+
+
+
+##
+end
+
+
+##