Merge pull request #1676 from JuliaRobotics/23Q1/enh/oaswip

minor enhancements

Author: dehann

src/ParametricUtils.jl

@@ -790,20 +790,10 @@ Update the parametric solver data value and covariance.
 """
 function updateSolverDataParametric! end
 
-function updateSolverDataParametric!(
-  v::DFGVariable,
-  val::AbstractArray{<:Real},
-  cov::Matrix;
-  solveKey::Symbol = :parametric,
-)
-  vnd = getSolverData(v, solveKey)
-  return updateSolverDataParametric!(vnd, val, cov)
-end
-
 function updateSolverDataParametric!(
   vnd::VariableNodeData,
-  val::AbstractArray{<:Real},
-  cov::Matrix,
+  val::AbstractArray,
+  cov::AbstractMatrix,
 )
   # fill in the variable node data value
   vnd.val[1] = val
@@ -812,6 +802,17 @@ function updateSolverDataParametric!(
   return vnd
 end
 
+function updateSolverDataParametric!(
+  v::DFGVariable,
+  val::AbstractArray,
+  cov::AbstractMatrix;
+  solveKey::Symbol = :parametric,
+)
+  vnd = getSolverData(v, solveKey)
+  return updateSolverDataParametric!(vnd, val, cov)
+end
+
+
 """
     $SIGNATURES
 Add parametric solver to fg, batch solve using [`solveGraphParametric`](@ref) and update fg.

src/services/FactorGraph.jl

@@ -864,7 +864,7 @@ end
 
 function DFG.addFactor!(
   dfg::AbstractDFG,
-  xisyms::AbstractVector{Symbol},
+  vlbs::AbstractVector{Symbol},
   usrfnc::AbstractFactor;
   suppressChecks::Bool = false,
   kw...,
@@ -873,11 +873,12 @@ function DFG.addFactor!(
 
   # basic sanity check for unary vs n-ary
   if !suppressChecks
-    _checkFactorAdd(usrfnc, xisyms)
+    _checkFactorAdd(usrfnc, vlbs)
+    @assert length(vlbs) == length(unique(vlbs)) "List of variables should be unique and ordered."
   end
 
-  # variables = getVariable.(dfg, xisyms)
-  variables = map(vid -> getVariable(dfg, vid), xisyms)
+  # variables = getVariable.(dfg, vlbs)
+  variables = map(vid -> getVariable(dfg, vid), vlbs)
   return addFactor!(dfg, variables, usrfnc; suppressChecks = suppressChecks, kw...)
 end