IncrementalInference.jl/IncrementalInference/src/parametric/services/ParametricRANSAC.jl at f16af6e23c03f89172c90b1f41009bce89e5068a · JuliaRobotics/IncrementalInference.jl · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122

#TODO upstream to DFG
function listNeighborhood(fg, variableFactorLabels, distance; filterOrphans=true)
    allvarfacs = getNeighborhood(fg, variableFactorLabels, distance)
    variableLabels = intersect(listVariables(fg), allvarfacs)
    factorLabels = intersect(listFactors(fg), allvarfacs)
    if filterOrphans
        filter!(factorLabels) do lbl
            issubset(getVariableOrder(fg, lbl), variableLabels)
        end
    end
    return  variableLabels, factorLabels
end

function calcResidualInliers(subfg, faclabels; kσ=1)
    varlabels =  setdiff(getNeighborhood(subfg, faclabels, 1), faclabels)

    vars = getVariable.(subfg, varlabels)
    M, varTypes, vartypeslist = IIF.buildGraphSolveManifold(vars)
    varIntLabel, varlabelsAP = IIF.getVarIntLabelMap(vartypeslist)

    p0 = map(varlabelsAP) do label
        getVal(subfg, label, solveKey = :parametric)[1]
    end

    # create an ArrayPartition{CalcFactorResidual} for faclabels
    calcfacs = IIF.CalcFactorResidualAP(subfg, faclabels, varIntLabel)

    # remember res = cf.sqrt_iΣ * factor res with calcfacs, so should be sigma scaled
    res = reduce(vcat, map(f -> f(p0), Vector(calcfacs)))
    # findfirst(==(median(res)), res)

    res_labels = getproperty.(Vector(calcfacs), :faclbl)
    # findfirst(==(:cD1l2000cD1l2000_cD1l5611f1), res_labels)

    # 3
    inlierlabels = deepcopy(res_labels)
    for (i,l) in (enumerate(faclabels))
        if abs(res[i]) > kσ
            setdiff!(inlierlabels, [l])
        end
    end

    return res_labels.=>res, inlierlabels
end

function solveGraphParametricRansac!(
    fg,
    comb_on_fac_labels,
    min_factors = 3,
    include_vars = Symbol[];
    n_iters = 50,
    stop_ratio = 0.7,
    kwargs...
)
    # intersect!(comb_on_fac_labels, listNeighbors(fg, ls(fg, r"^x")[1]))
    all_fac_combinations = combinations(comb_on_fac_labels, min_factors)

    do_combinations = length(all_fac_combinations) < n_iters ?
        collect(all_fac_combinations) : rand(collect(all_fac_combinations), n_iters)
        # collect(all_fac_combinations) : all_fac_combinations[rand(1:length(all_fac_combinations), n_iters)]

    best_ratio = 0.0
    best_inlierlabels = Symbol[]
    for (i, maybe_inliers) = enumerate(do_combinations)

        solveVariableLabels, solveFactorLabels = listNeighborhood(fg, union(include_vars, maybe_inliers), 2)

        # TODO do better
        subfg = deepcopy(fg)
        # @info solveFactorLabels
        # M, v, r, Σ = IIF.solve_RLM(fg, variableLabels, factorLabels;
        try
            IIF.solveGraphParametric!(subfg, solveVariableLabels, solveFactorLabels; kwargs...)
            # IIF.solveGraphParametric!(subfg, kwargs...)
        catch er
            @warn "Error on iter $i" er
            continue
        end

        residuals, inlierlabels = calcResidualInliers(subfg, comb_on_fac_labels)

        ratio_inliers = length(inlierlabels) ./ length(comb_on_fac_labels)

        if ratio_inliers > best_ratio
            best_ratio = ratio_inliers
            @info "new best $best_ratio"
            best_inlierlabels = inlierlabels
        end
        if ratio_inliers > stop_ratio
            @info "stop ratio with $best_ratio"
            break
        end
        # res_vals = last.(residuals)

    end
    try
        solveVariableLabels, solveFactorLabels = listNeighborhood(fg, union(include_vars, best_inlierlabels), 2)
        IIF.solveGraphParametric!(fg, solveVariableLabels, solveFactorLabels; kwargs...)
    catch er
        @error "solveGraphParametric of inliers failed" er
    end

    return best_inlierlabels
end

if false
## get factor residuals to solve with
comb_on_fac_labels = lsfTypesDict(subfg)[:Pose2Point2Bearing]
# intersect!(faclabels, listNeighbors(subfg, ls(subfg, r"^cD1l\d+$")[1]))
stopping_criterion=StopAfterIteration(100) | StopWhenGradientNormLess(1e-12) | StopWhenStepsizeLess(1e-12)

inlierlabels = solveGraphParametricRansac!(subfg, comb_on_fac_labels;
    n_iters = 500,
    stopping_criterion,
    # debug,
    is_sparse=false,
    damping_term_min=1e-12,
    finiteDiffCovariance=true
)

end