Update VelAlign.jl

JuliaRobotics · Aug 26, 2024 · 623edfb · 623edfb
1 parent 2b5010b
commit 623edfb
Showing 1 changed file with 2 additions and 1 deletion.
diff --git a/src/factors/VelAlign.jl b/src/factors/VelAlign.jl
@@ -35,6 +35,7 @@ function (cf::CalcFactor{<:VelAlign})(
   p_R_q
 )
   # body velocity scaled by real speed
+  # FIXME align directions and adaptively weight by speed (i.e. slow is less important)
   p_V = norm(cf.cache.p_vel(w_T_p)) .* X_v
   q_V = cf.cache.q_rot(w_T_q)' * cf.cache.q_vel(w_T_q)
   p_V - p_R_q * q_V
@@ -54,4 +55,4 @@ function convert(::Type{VelAlign}, d::PackedVelAlign)
 end
 function convert(::Type{PackedVelAlign}, d::VelAlign)
   return PackedVelAlign( convert(PackedSamplableBelief, d.Z) )
-end
+end