Skip to content

Commit

Permalink
Merge pull request #732 from JuliaRobotics/24Q1/enh/docsonbearing
Browse files Browse the repository at this point in the history
Add docs on Bearing2D.jl
  • Loading branch information
dehann authored Mar 7, 2024
2 parents f7347f4 + b6e374e commit 86171a6
Showing 1 changed file with 3 additions and 2 deletions.
5 changes: 3 additions & 2 deletions src/factors/Bearing2D.jl
Original file line number Diff line number Diff line change
Expand Up @@ -24,9 +24,10 @@ function (cfo::CalcFactor{<:Pose2Point2Bearing})(X, p, l)
# wl = l
# wTp = p
# pl = pTw*wl

pl = transpose(p.x[2]) * (l - p.x[1])
# δθ = mθ - plθ
δθ = Manifolds.sym_rem(X[2] - atan(pl[2], pl[1]))
# δθ = mθ - plθ # X[2,1] because we store [measurement tangent as 2x2 matrix (Lie algebra, so(2))](https://juliamanifolds.github.io/Manifolds.jl/stable/manifolds/group.html#Manifolds.exp_lie-Tuple{Manifolds.GeneralUnitaryMultiplicationGroup{ManifoldsBase.TypeParameter{Tuple{2}},%20%E2%84%9D},%20Any})
δθ = Manifolds.sym_rem(X[2,1] - atan(pl[2], pl[1]))
return [δθ]
end

Expand Down

0 comments on commit 86171a6

Please sign in to comment.