@@ -515,12 +515,18 @@ function _totalCost(fg,
515515end
516516
517517
518- # TODO maybe consolidate with solveGraphParametric
519- # TODO WIP
520- ```
521- $SIGNATURES
518+ """
519+ $SIGNATURES
522520Solve for frontal values only with values in seprarators fixed
523- ```
521+
522+ DevNotes
523+ - WIP
524+ - Relates to: https://github.com/JuliaRobotics/IncrementalInference.jl/issues/466#issuecomment-562556953
525+ - Consolidation
526+ - Definitely with [`solveFactorParameteric`](@ref)
527+ - Maybe with [`solveGraphParametric`](@ref)
528+ - https://github.com/JuliaRobotics/IncrementalInference.jl/pull/1588#issuecomment-1210406683
529+ """
524530function solveConditionalsParametric (fg:: AbstractDFG ,
525531 frontals:: Vector{Symbol} ,
526532 separators:: Vector{Symbol} = setdiff (listVariables (fg), frontals);
@@ -769,170 +775,5 @@ function createMvNormal(v::DFGVariable, key=:parametric)
769775end
770776
771777
772- function autoinitParametric! (dfg:: AbstractDFG ,
773- initme:: Symbol ;
774- reinit:: Bool = false )
775- #
776- solveKey = :parametric
777-
778- xi = getVariable (dfg, initme)
779- vnd = getSolverData (xi, solveKey)
780- # don't initialize a variable more than once
781- if reinit || ! isInitialized (xi, solveKey)
782-
783- # frontals - initme
784- # separators - inifrom
785-
786- initfrom = ls2 (dfg, initme)
787- filter! (initfrom) do vl
788- isInitialized (dfg, vl, solveKey)
789- end
790-
791- vardict, result, flatvars, Σ = solveConditionalsParametric (dfg, [initme], initfrom)
792-
793- val,cov = vardict[initme]
794-
795- # fill in the variable node data value
796- vnd. val[1 ] = val
797- # calculate and fill in covariance
798- vnd. bw = cov
799-
800- vnd. initialized = true
801- # fill in ppe as mean
802- Xc = getCoordinates (getVariableType (xi), val)
803- ppe = MeanMaxPPE (:parametric , Xc, Xc, Xc)
804- getPPEDict (xi)[:parametric ] = ppe
805-
806- # updateVariableSolverData!(dfg, xi, solveKey, true; warn_if_absent=false)
807- # updateVariableSolverData!(dfg, xi.label, getSolverData(xi, solveKey), :graphinit, true, Symbol[]; warn_if_absent=false)
808- else
809- result = nothing
810- end
811-
812- return result# isInitialized(xi, solveKey)
813- end
814-
815- # # ================================================================================================
816- # # Experimental specialized dispatch for Mixture
817- # # ================================================================================================
818- # To sort out how to dispatch on specialized functions.
819- # related to #931 and #1069
820-
821- struct MaxMixture <: AbstractMaxMixtureSolver
822- p:: Vector{Float64}
823- # the chosen component to be used for the optimization
824- choice:: Base.RefValue{Int}
825- end
826-
827- function getMeasurementParametric (s:: Mixture{N,F,S,T} ) where {N,F,S,T}
828- meas = map (c-> getMeasurementParametric (c)[1 ], values (s. components))
829- iΣ = map (c-> getMeasurementParametric (c)[2 ], values (s. components))
830- return meas, iΣ
831- end
832-
833- function _calcFactorMahalanobis (cfp, meas, iΣ, variables... )
834- res = cfp. calcfactor! (meas, variables... )
835- r = res' * iΣ * res
836- return r
837- end
838-
839- # DEV NOTE: function with other options including select once and use
840- # function (cfp::CalcFactorMahalanobis{<:CalcFactor, MaxMixture})(variables...)
841- # if cfp.specialAlg.choice[] == 0
842- # #calculate all mixture options
843- # r = [_calcFactorMahalanobis(cfp, cfp.meas[i], cfp.iΣ[i], variables...) for i = 1:length(cfp.meas)]
844-
845- # p = cfp.specialAlg.p
846778
847- # k = size(cfp.iΣ[1], 2)
848- # # α = 1 ./ sqrt.(2pi .* k .* det.(inv.(cfp.iΣ)))
849- # α = sqrt.(det.(cfp.iΣ) ./ ((2pi)^k))
850-
851- # # mm, at = findmax(α .* p .* exp.(-0.5 .* r))
852- # # mm = sum(α .* p .* exp.(-0.5 .* r) )
853-
854- # mm, at = findmin( 0.5 .* r .- log.(α .* p))
855- # # mm = -log(sum(α .* p .* exp.(-0.5 .* r) ))
856- # # return mm + maximum(log.(α .* p))
857-
858- # cfp.specialAlg.choice[] = at
859-
860- # return r[at]
861-
862- # else
863- # at = cfp.specialAlg.choice[]
864- # return _calcFactorMahalanobis(cfp, cfp.meas[at], cfp.iΣ[at], variables...)
865- # end
866-
867- # end
868-
869- function (cfp:: CalcFactorMahalanobis{N, MaxMixture} )(variables... ) where N
870-
871- r = [_calcFactorMahalanobis (cfp, cfp. meas[i], cfp. iΣ[i], variables... ) for i = 1 : length (cfp. meas)]
872-
873- p = cfp. specialAlg. p
874-
875- k = size (cfp. iΣ[1 ], 2 )
876- # α = 1 ./ sqrt.(2pi .* k .* det.(inv.(cfp.iΣ)))
877- α = sqrt .(det .(cfp. iΣ) ./ ((2pi )^ k))
878-
879- mm, at = findmin (r .- log .(α .* p))
880- # mm = -log(sum(α .* p .* exp.(-0.5 .* r) ))
881- return mm + maximum (log .(α .* p))
882-
883- end
884-
885-
886- # # ================================================================================================
887- # # Experimental specialised dispatch for multihypo and nullhypo
888- # # ================================================================================================
889- # TODO better dispatch
890-
891- struct MaxMultihypo <: AbstractMaxMixtureSolver
892- multihypo:: Vector{Float64}
893- end
894- struct MaxNullhypo <: AbstractMaxMixtureSolver
895- nullhypo:: Float64
896- end
897-
898- function (cfp:: CalcFactorMahalanobis{N, MaxMultihypo} )(X1, L1, L2) where N
899- mh = cfp. specialAlg. multihypo
900- @assert length (mh) == 3 " multihypo $mh not supported with parametric, length should be 3"
901- @assert mh[1 ] == 0 " multihypo $mh not supported with parametric, first should be 0"
902-
903- # calculate both multihypo options
904- r1 = cfp (X1, L1)
905- r2 = cfp (X1, L2)
906- r = [r1, r2]
907-
908- # hacky multihypo to start of with
909- mm, at = findmin (r .* (1 .- mh[2 : end ]))
910- nat = at == 1 ? 1 : 2
911- k = length (X1)* one (r1) * 1e-3
912- return r[at] + r[nat]* k
913-
914- end
915-
916- function (cfp:: CalcFactorMahalanobis{N, MaxNullhypo} )(X1, X2) where N
917- nh = cfp. specialAlg. nullhypo
918- @assert nh > 0 " nullhypo $nh not as expected"
919-
920- # calculate factor residual
921- res = cfp. calcfactor! (cfp. meas[1 ], X1, X2)
922- r1 = res' * cfp. iΣ * res
923-
924- # compare to uniform nullhypo
925- r2 = length (res)* one (r1)
926- r = [r1,r2]
927- mm, at = findmin (r .* [nh, (1 - nh)])
928-
929- residual = at == 1 ? r1 : r1* 1e-3
930-
931- return residual
932-
933- # rand residual option
934- # idx = rand(Categorical([(1-nh), nh]))
935- # nh == 0.05 && cfp.varOrder==[:x1,:l1] && println("$idx -> $(r1.value), $r2")
936- # return r[idx]
937-
938- end
779+ #
0 commit comments