Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

fix pose2pose2 gto #636

Merged
merged 4 commits into from
Oct 29, 2022
Merged

fix pose2pose2 gto #636

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5a4ee1a
fix pose2pose2 gto
Affie Oct 3, 2022
a763fb2
kwarg varIntLabels as OrderedDict in exportG2o
Affie Oct 10, 2022
16836e1
g2o export: optional VERTEX_SE2
Affie Oct 28, 2022
efc96a8
fix g2o test?
Affie Oct 28, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Jump to
Jump to file
Failed to load files.
Diff view
Diff view
2 changes: 2 additions & 0 deletions Project.toml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@ LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
Manifolds = "1cead3c2-87b3-11e9-0ccd-23c62b72b94e"
ManifoldsBase = "3362f125-f0bb-47a3-aa74-596ffd7ef2fb"
Optim = "429524aa-4258-5aef-a3af-852621145aeb"
OrderedCollections = "bac558e1-5e72-5ebc-8fee-abe8a469f55d"
ProgressMeter = "92933f4c-e287-5a05-a399-4b506db050ca"
Random = "9a3f8284-a2c9-5f02-9a11-845980a1fd5c"
Reexport = "189a3867-3050-52da-a836-e630ba90ab69"
Expand All @@ -45,6 +46,7 @@ KernelDensityEstimate = "0.5.1, 0.6"
Manifolds = "0.8.11"
ManifoldsBase = "0.13"
Optim = "0.22, 1.0"
OrderedCollections = "1"
ProgressMeter = "1"
Reexport = "0.2, 1.0"
Requires = "1.0"
Expand Down
1 change: 1 addition & 0 deletions src/RoME.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,7 @@ import IncrementalInference: InstanceType
import DistributedFactorGraphs: compare
import DistributedFactorGraphs: getDimension, getManifold

using OrderedCollections: OrderedDict
# const AMP = ApproxManifoldProducts


Expand Down
40 changes: 29 additions & 11 deletions src/services/g2oParser.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -171,7 +171,7 @@ end
function stringG2o!(dfg::AbstractDFG,
fc::Symbol,
fnc::Pose2Pose2,
varIntLabel::Dict,
varIntLabel::OrderedDict,
uniqVarInt::Vector{Int};
overwriteMapping::Dict=Dict())::String
#
Expand All @@ -183,13 +183,13 @@ function stringG2o!(dfg::AbstractDFG,
INF[INF .== Inf] .= 0
# get command
comm = !haskey(overwriteMapping, Pose2Pose2) ? commands[Pose2Pose2] : overwriteMapping[Pose2Pose2]
return "$comm $(varlist[1]) $(varlist[2]) $(fnc.Z.μ[1]) $(fnc.Z.μ[2]) $(fnc.Z.μ[3]) $(INF[1,1]) $(INF[1,1]) $(INF[1,2]) $(INF[1,3]) $(INF[2,2]) $(INF[2,3]) $(INF[2,3]) $(INF[2,3])"
return "$comm $(varlist[1]) $(varlist[2]) $(fnc.Z.μ[1]) $(fnc.Z.μ[2]) $(fnc.Z.μ[3]) $(INF[1,1]) $(INF[1,2]) $(INF[1,3]) $(INF[2,2]) $(INF[2,3]) $(INF[3,3])"
end

function stringG2o!(dfg::AbstractDFG,
fc::Symbol,
fnc::Pose2Point2BearingRange,
varIntLabel::Dict,
varIntLabel::OrderedDict,
uniqVarInt::Vector{Int};
overwriteMapping::Dict=Dict{Symbol,Symbol}())::String
#
Expand All @@ -206,7 +206,7 @@ end
function stringG2o!(dfg::AbstractDFG,
fc::Symbol,
fnc,
varIntLabel::Dict,
varIntLabel::OrderedDict,
uniqVarInt::Vector{Int};
overwriteMapping::Dict=Dict{Symbol,Symbol}())
#
Expand All @@ -222,15 +222,18 @@ Export a factor graph to g2o file format.
Note:
- This funtion only supports Gaussian (i.e. Normal/MvNormal) factors, unpredictable witchcraft is used in other cases such as `AliasingScalarSampler` factor models.
"""
function exportG2o(dfg::AbstractDFG;
poseRegex::Regex=r"x\d",
solvable::Int=0,
ignorePriors::Bool=true,
filename::AbstractString="/tmp/test.txt",
overwriteMapping::Dict=Dict{Symbol, Symbol}())
function exportG2o(
dfg::AbstractDFG;
poseRegex::Regex=r"x\d",
solvable::Int=0,
ignorePriors::Bool=true,
filename::AbstractString="/tmp/test.txt",
overwriteMapping::Dict=Dict{Symbol, Symbol}(),
varIntLabel::OrderedDict{Symbol, Int} = OrderedDict{Symbol, Int}(),
solveKey::Union{Nothing,Symbol}=nothing,
)
#
uniqVarInt = Int[-1;]
varIntLabel = Dict{Symbol, Int}()
# all variables
vars = ls(dfg, poseRegex, solvable=solvable) |> sortDFG
# all factors
Expand All @@ -255,6 +258,21 @@ function exportG2o(dfg::AbstractDFG;
end
close(io)

if !isnothing(solveKey)
open(filename, "a") do io
for (label,i) in pairs(varIntLabel)
vartype = getVariableType(dfg, label)
if vartype === Pose2()
(x,y,θ) = getPPESuggested(dfg, label, solveKey)
write(io, "VERTEX_SE2 $i $x $y $θ\n")
else
#TODO variable type Pose2 Point2 Pose3(VERTEX_SE3:QUAT ID x y z q_x q_y q_z q_w) Point3
error("exportG2o does not support $vartype, open an issue if you would like support")
end
end
end
end

return filename
end

Expand Down
12 changes: 6 additions & 6 deletions test/testG2oParser.jl
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -26,13 +26,13 @@ end
@testset "Test g2o file parsing." begin

reflines = [
"EDGE_SE2 0 1 10.0 0.0 1.0471975511965976 10.0 10.0 0.0 0.0 10.0 0.0 0.0 0.0";
"EDGE_SE2 0 1 10.0 0.0 1.0471975511965976 10.0 0.0 0.0 10.0 0.0 10.0";
"LANDMARK 0 2 0.0 20.0 3.162277660168379 0.0 1.0";
"EDGE_SE2 1 3 10.0 0.0 1.0471975511965976 10.0 10.0 0.0 0.0 10.0 0.0 0.0 0.0";
"EDGE_SE2 3 4 10.0 0.0 1.0471975511965976 10.0 10.0 0.0 0.0 10.0 0.0 0.0 0.0";
"EDGE_SE2 4 5 10.0 0.0 1.0471975511965976 10.0 10.0 0.0 0.0 10.0 0.0 0.0 0.0";
"EDGE_SE2 5 6 10.0 0.0 1.0471975511965976 10.0 10.0 0.0 0.0 10.0 0.0 0.0 0.0";
"EDGE_SE2 6 7 10.0 0.0 1.0471975511965976 10.0 10.0 0.0 0.0 10.0 0.0 0.0 0.0";
"EDGE_SE2 1 3 10.0 0.0 1.0471975511965976 10.0 0.0 0.0 10.0 0.0 10.0";
"EDGE_SE2 3 4 10.0 0.0 1.0471975511965976 10.0 0.0 0.0 10.0 0.0 10.0";
"EDGE_SE2 4 5 10.0 0.0 1.0471975511965976 10.0 0.0 0.0 10.0 0.0 10.0";
"EDGE_SE2 5 6 10.0 0.0 1.0471975511965976 10.0 0.0 0.0 10.0 0.0 10.0";
"EDGE_SE2 6 7 10.0 0.0 1.0471975511965976 10.0 0.0 0.0 10.0 0.0 10.0";
Comment on lines +29 to +35
Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@dehann , can you please just confirm that this test is now correct as you wrote the last one?

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think its correct: https://github.com/RainerKuemmerle/g2o/wiki/File-Format#edges--constraints

"LANDMARK 7 2 0.0 20.0 3.162277660168379 0.0 1.0"]


Expand Down