Skip to content

Confirm what log (or parameterization/metric) to use in PriorPose2 #615

@Affie

Description

@Affie

# TODO the log here looks wrong (for gradients), consider:
# X = log(p⁻¹ ∘ m)
# X = log(M, ϵ, Manifolds.compose(M, inv(M, p), m))
function (cf::CalcFactor{<:PriorPose2})(
m::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}},
p::ArrayPartition{T, Tuple{SVector{2, T}, SMatrix{2, 2, T, 4}}}) where T<:Real
M = getManifold(Pose2)
Xc = _vee(M, log(M, p, m))
return Xc
end

Metadata

Metadata

Assignees

Type

No type

Projects

No projects

Relationships

None yet

Development

No branches or pull requests

Issue actions