From b6e374e3a728b77f7768a55829586cba9ef56ee0 Mon Sep 17 00:00:00 2001
From: Dehann Fourie <6412556+dehann@users.noreply.github.com>
Date: Thu, 29 Feb 2024 10:03:57 -0800
Subject: [PATCH] Add docs on Bearing2D.jl

closes:
- #731
---
 src/factors/Bearing2D.jl | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/factors/Bearing2D.jl b/src/factors/Bearing2D.jl
index 3c310a7e..fbccd296 100644
--- 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