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

New Factor Pose3Pose3UnitTrans #734

Merged
merged 1 commit into from Mar 25, 2024
Merged

New Factor Pose3Pose3UnitTrans #734

Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
23 changes: 18 additions & 5 deletions src/factors/Pose3Pose3.jl
View file
Open in desktop
Expand Up @@ -47,11 +47,6 @@
return PackedPose3Pose3( convert(PackedSamplableBelief, obj.Z) )
end




#

##
Base.@kwdef struct Pose3Pose3RotOffset{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)]))
Expand All @@ -73,3 +68,21 @@
q̂ = Manifolds.compose(M, p, b_m)
return vee(M, q, log(M, q, q̂)) # coordinates
end

## Pose3Pose3UnitTrans Factor
"""
$(TYPEDEF)
Pose3Pose3 factor where the translation scale is not known, ie. Pose3Pose3 with unit (normalized) translation.
"""
Base.@kwdef struct Pose3Pose3UnitTrans{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)]))
end

getManifold(::InstanceType{Pose3Pose3UnitTrans}) = getManifold(Pose3) # Manifolds.SpecialEuclidean(3)

Check warning on line 81 in src/factors/Pose3Pose3.jl

View check run for this annotation

Codecov / codecov/patch

src/factors/Pose3Pose3.jl#L81 

Added line #L81 was not covered by tests

function (cf::CalcFactor{<:Pose3Pose3UnitTrans})(X, p::ArrayPartition{T}, q) where T
M = getManifold(Pose3)
q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X))
Xc::SVector{6,T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis())
return SVector{6,T}(normalize(Xc[1:3])..., Xc[4:6]...)

Check warning on line 87 in src/factors/Pose3Pose3.jl

View check run for this annotation

Codecov / codecov/patch

src/factors/Pose3Pose3.jl#L83-L87 

Added lines #L83 - L87 were not covered by tests
end