new utils, better code placement

NEWS.md

@@ -6,6 +6,8 @@ RoME.jl follows semver, with only a few case specific exceptions.  Please see re
 
 - Add `SnoopPrecompile` and Julia v1.8 min compat.
 - Remove `FactorMetadata` and `ConvPerThread` usage as per IIF v0.31.
+- Move code and files to subfolders `services`, `entities`, and `legacy`.
+- Added new features `homograph_to_coordinates` and `coordinates_to_homography`.
 
 ## v0.20
 

Project.toml

@@ -2,7 +2,7 @@ name = "RoME"
 uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.21.0"
+version = "0.21.1"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

src/Deprecated.jl

@@ -1,14 +1,36 @@
 
 ##==============================================================================
-## Legacy, remove some time after RoME v0.21
+## Legacy, remove some time after RoME v0.22
 ##==============================================================================
 
+@deprecate homographyToCoordinates(w...;kw...) homography_to_coordinates(w...;kw...)
+
+export measureMeanDist
+function measureMeanDist(fg::AbstractDFG, a::AbstractString, b::AbstractString)
+  @error "RoME.measureMeanDist is obsolete"
+  #bearrang!(residual::Array{Float64,1}, Z::Array{Float64,1}, X::Array{Float64,1}, L::Array{Float64,1})
+  res = zeros(2)
+  A = getVal(fg,a)
+  B = getVal(fg,b)
+  Ax = Statistics.mean(vec(A[1,:]))
+  Ay = Statistics.mean(vec(A[2,:]))
+  Bx = Statistics.mean(vec(B[1,:]))
+  By = Statistics.mean(vec(B[2,:]))
+  dx = Bx - Ax
+  dy = By - Ay
+  b = atan(dy,dx)
+  r = sqrt(dx^2 + dy^2)
+  return r, b
+end
+
+# should be deprecated or indicated more clearly
+@deprecate lsrBR(a) [a[2,:];a[1,:]]'
+
 # import AMP: _makeVectorManifold
 # AMP._makeVectorManifold(::M, prr::ProductRepr) where {M <: typeof(BearingRange_Manifold)} = coords(M, prr)
 
 
 
-
 ##==============================================================================
 ## Remove as part of Manifolds.jl consolidation, #244
 ##==============================================================================

src/ExportAPI.jl

@@ -146,7 +146,6 @@ export
   rangeCompAllPoses,
 
   # older features
-  measureMeanDist,
   predictBodyBR,
   calcPosePointBearingRange,
   odomKDE,
@@ -208,12 +207,10 @@ export
   measUpdateTrackers!,
   assocMeasWFeats!,
 
-  lsrBR,
-
   # jld required Features Type
   LaserFeatures
 
-
+export listVariablesLabelsWithinRange, enableSolveAllNotDRT!
 
 # from odometry utils
 export getMeasurementParametric
@@ -234,4 +231,6 @@ export
   PackedInertialPose3,
   PriorInertialPose3,
   PackedPriorInertialPose3,
-  compare
+  compare
+
+export homography_to_coordinates, coordinates_to_homography

src/RoME.jl

@@ -27,7 +27,7 @@ using SnoopPrecompile
 
 # to avoid name conflicts
 import Manifolds
-using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, ProductRepr, SpecialOrthogonal, TranslationGroup, identity_element, submanifold_component
+using Manifolds: hat, ProductGroup, ProductManifold, SpecialEuclidean, ProductRepr, SpecialOrthogonal, TranslationGroup, identity_element, submanifold_component, Identity, affine_matrix
 
 import Manifolds: project, project!
 
@@ -58,10 +58,10 @@ include("ExportAPI.jl")
 
 
 # load the source files
-include("SpecialDefinitions.jl")
+include("entities/SpecialDefinitions.jl")
 
 #uses DFG v0.10.2 @defVariable for above
-include("variables/Local_Manifold_Workaround.jl")
+include("services/FixmeManifolds.jl")
 include("variables/VariableTypes.jl")
 
 ## More factor types
@@ -87,20 +87,24 @@ include("factors/Pose3Pose3.jl")
 include("factors/PartialPose3.jl")
 include("factors/MultipleFeaturesConstraint.jl")
 include("factors/InertialPose3.jl")
+# needs maintenance
+include("factors/RangeAzimuthElevation.jl")
 
 # additional tools
-include("FactorGraphAnalysisTools.jl")
+include("services/FactorGraphAnalysisTools.jl")
 
 # tools related to robotics
-include("BayesTracker.jl")
-include("SensorModels.jl")
-include("CameraModel.jl")
-include("Slam.jl")
-include("RobotUtils.jl")
-include("SimulationUtils.jl")
-include("OdometryUtils.jl")
-include("RobotDataTypes.jl")
-include("NavigationSystem.jl")
+include("legacy/BayesTracker.jl")
+include("factors/SensorModels.jl")
+include("legacy/CameraModel.jl")
+include("legacy/Slam.jl")
+include("services/RobotUtils.jl")
+include("services/ManifoldUtils.jl")
+include("services/BearingRangeUtils.jl")
+include("services/SimulationUtils.jl")
+include("services/OdometryUtils.jl")
+include("entities/RobotDataTypes.jl")
+include("legacy/NavigationSystem.jl")
 
 # generate canonical graphs
 include("canonical/GenerateCommon.jl")
@@ -112,8 +116,8 @@ include("canonical/GenerateBeehive.jl")
 include("canonical/GenerateHelix.jl")
 
 # more utils requiring earlier functions
-include("AdditionalUtils.jl")
-include("g2oParser.jl")
+include("services/AdditionalUtils.jl")
+include("services/g2oParser.jl")
 
 # ScalarFields
 include("services/ScalarFields.jl")

src/factors/RangeAzimuthElevation.jl

@@ -0,0 +1,29 @@
+
+# FIXME update to Manifolds.Sphere instead
+
+mutable struct RangeAzimuthElevation
+  range::Float64
+  azimuth::Float64
+  elevation::Union{Nothing,Float64}
+end
+
+
+function convert(::Type{RangeAzimuthElevation}, val::Tuple{Symbol, Vector{Float64}})
+  if val[1] == :rangeazimuth
+    return RangeAzimuthElevation(val[2][1],val[2][2],nothing)
+  elseif val[1] == :rangeazimuthelevation
+    return RangeAzimuthElevation(val[2][1],val[2][2],val[2][3])
+  else
+    error("Unknown conversion from $(val[1]) to RangeAzimuthElevation.")
+  end
+end
+
+
+function \(s::SE3, wTr::CTs.Translation)
+  bTr = s.R.R'*(wTr.v-s.t)
+  Dtr = bTr
+  range = norm(Dtr)
+  azi = atan(Dtr[2], Dtr[1])
+  elev = atan(Dtr[3], Dtr[1])
+  RangeAzimuthElevation(range, azi, elev)
+end

src/CameraModel.jl → src/legacy/CameraModel.jl

@@ -1,4 +1,5 @@
 
+## FIXME, refactor to use JuliaRobotics/CameraModels.jl instead
 mutable struct CameraIntrinsic
   K::Array{Float64,2}
   CameraIntrinsic(::Nothing) = new()

src/NavigationSystem.jl → src/legacy/NavigationSystem.jl

@@ -3,24 +3,6 @@
 # standardize a common front-end design with feature tracking and real-time integration
 # part of DFG discussion
 
-function triggerPose(x, xprev, Tnow, Tprev,
-                    distrule, timerule, yawrule)
-
-  if norm(x[1:2]-xprev[1:2]) >= distrule
-    @show Tnow, round(x,digits=2), round(xprev,digits=2)
-    @show norm(x[1:2]-xprev[1:2])
-    return 1
-  end
-  if abs(x[3]-xprev[3]) >= yawrule
-    @show Tnow, round(x,digits=2), round(xprev,digits=2)
-    @show abs(x[3]-xprev[3])
-    return 2
-  end
-  if Tnow-Tprev > timerule
-    return 3
-  end
-  return 0
-end
 
 mutable struct GenericInSituSystem{T}
   xprev::Array{Float64,1}
@@ -113,7 +95,7 @@ function poseTrigAndAdd!(instSys::InSituSystem, Ts::Float64,
                         distrule::Float64, timerule::Float64, yawrule::Float64;
                         xprev=zeros(3), auxtrig::Bool=false)
 
-  error(" Yeah dehann, do it properly")
+  error(" Yeah, do it properly")
 
 
 end

src/services/BearingRangeUtils.jl

@@ -0,0 +1,90 @@
+
+
+function predictBodyBR(fg::AbstractDFG, a::Symbol, b::Symbol)
+  res = zeros(2)
+  A = getKDEMean(getBelief(getVariable(fg,a)))
+  B = getKDEMean(getBelief(getVariable(fg,b)))
+  Ax = A[1] # Statistics.mean(vec(A[1,:]))
+  Ay = A[2] # Statistics.mean(vec(A[2,:]))
+  Ath = getKDEMax(getBelief(getVariable(fg,a)))[3]
+  Bx = B[1]
+  By = B[2]
+  wL = SE2([Bx;By;0.0])
+  wBb = SE2([Ax;Ay;Ath])
+  bL = se2vee((wBb \ Matrix{Float64}(LinearAlgebra.I, 3,3)) * wL)
+  dx = bL[1] - 0.0
+  dy = bL[2] - 0.0
+  b = (atan(dy,dx))
+  r = sqrt(dx^2 + dy^2)
+  return b, r
+end
+
+
+function malahanobisBR(measA, preA, cov::Array{Float64,2})
+  # measure landmark with noise
+  res = measA - preA
+  mala2 = Union{}
+  #Malahanobis distance
+  if false
+    lambda = cov \ Matrix{Float64}(LinearAlgebra.I, 2,2)
+    mala2 = res' * lambda * res
+  else
+    mala2 = res' * (cov \ res)
+  end
+
+  mala = sqrt(mala2)
+  return mala
+end
+
+
+
+"""
+    $SIGNATURES
+
+Method to compare current and predicted estimate on a variable, developed for testing a new factor before adding to the factor graph.
+
+Notes
+- `fct` does not have to be in the factor graph -- likely used to test beforehand.
+- function is useful for detecting if `multihypo` should be used.
+- `approxConv` will project the full belief estimate through some factor but must already be in factor graph.
+
+Example
+
+```julia
+# fg already exists containing :x7 and :l3
+pp = Pose2Point2BearingRange(Normal(0,0.1),Normal(10,1.0))
+# possible new measurement from :x7 to :l3
+curr, pred = predictVariableByFactor(fg, :l3, pp, [:x7; :l3])
+# example of naive user defined test on fit score
+fitscore = minkld(curr, pred)
+# `multihypo` can be used as option between existing or new variables
+```
+
+Related
+
+approxConv
+"""
+function predictVariableByFactor( dfg::AbstractDFG,
+                                  targetsym::Symbol,
+                                  fct::Pose2Point2BearingRange,
+                                  prevars::Vector{Symbol}  )
+  #
+  @assert targetsym in prevars
+  curr = getBelief(dfg, targetsym)
+  tfg = initfg()
+  for var in prevars
+    varnode = getVariable(dfg, var)
+    addVariable!(tfg, var, getSofttype(varnode))
+    if var != targetsym
+      @assert isInitialized(varnode)
+      initVariable!(tfg,var,getBelief(varnode))
+    end
+  end
+  addFactor!(tfg, prevars, fct, graphinit=false)
+  fctsym = ls(tfg, targetsym)
+
+  pts, infd = predictbelief(tfg, targetsym, fctsym)
+  pred = manikde!(getManifold(getVariable(dfg, targetsym)), pts)
+  # return current and predicted beliefs
+  return curr, pred
+end

src/services/ManifoldUtils.jl

@@ -0,0 +1,49 @@
+
+function homography_to_coordinates(
+  M::typeof(SpecialEuclidean(3)),
+  pHq::AbstractMatrix{<:Real}
+)
+  Mr = M.manifold[2]
+  e0 = Identity(Mr)
+  [pHq[1:3,4]; vee(Mr, e0, log(Mr, e0, pHq[1:3,1:3]))]
+end
+
+function coordinates_to_homography(
+  M::typeof(SpecialEuclidean(3)),
+  pCq::AbstractVector
+)
+  e0 = Identity(M)
+  affine_matrix(M, exp(M,e0,hat(M,e0,pCq)))
+end
+
+
+## =============================================
+## Legacy code below
+## =============================================
+
+
+function convert(::Type{_Rot.QuatRotation}, q::TransformUtils.Quaternion)
+  _Rot.QuatRotation(q.s, q.v...)
+end
+function convert(::Type{_Rot.QuatRotation}, x::SO3)
+  q = convert(TransformUtils.Quaternion, x)
+  convert(_Rot.QuatRotation, q)
+end
+function convert(::Type{T}, x::SO3) where {T <: CoordinateTransformations.AffineMap}
+  LinearMap( convert(_Rot.QuatRotation, x) )
+end
+
+function convert(::Type{T}, x::SE3) where {T <: CoordinateTransformations.AffineMap}
+  Translation(x.t...) ∘ convert(AffineMap{_Rot.QuatRotation{Float64}}, x.R)
+end
+function convert(::Type{SE3}, x::T) where {T <: CoordinateTransformations.AffineMap{_Rot.QuatRotation{Float64}}}
+  SE3(x.translation[1:3], TransformUtils.Quaternion(x.linear.w, [x.linear.x,x.linear.y,x.linear.z]) )
+end
+
+function convert(::Type{SE3}, t::Tuple{Symbol, Vector{Float64}})
+  if t[1]==:XYZqWXYZ
+    return SE3(t[2][1:3],TransformUtils.Quaternion(t[2][4],t[2][5:7]))
+  else
+    error("Unknown conversion type $(t[1])")
+  end
+end