Merge pull request #732 from JuliaRobotics/24Q1/enh/docsonbearing

Add docs on Bearing2D.jl
JuliaRobotics · Mar 7, 2024 · 86171a6 · 86171a6
2 parents f7347f4 + b6e374e
commit 86171a6
Showing 1 changed file with 3 additions and 2 deletions.
diff --git a/src/factors/Bearing2D.jl b/src/factors/Bearing2D.jl
@@ -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