Merge pull request #736 from JuliaRobotics/master

v0.24.3 rc1

Project.toml

@@ -2,7 +2,7 @@ name = "RoME"
 uuid = "91fb55c2-4c03-5a59-ba21-f4ea956187b8"
 keywords = ["SLAM", "state-estimation", "MM-iSAM", "MM-iSAMv2", "inference", "robotics"]
 desc = "Non-Gaussian simultaneous localization and mapping"
-version = "0.24.2"
+version = "0.24.3"
 
 [deps]
 ApproxManifoldProducts = "9bbbb610-88a1-53cd-9763-118ce10c1f89"

src/factors/Bearing2D.jl

@@ -19,18 +19,15 @@ preambleCache(::AbstractDFG, ::AbstractVector{<:DFGVariable}, ::Pose2Point2Beari
 
 getManifold(::Pose2Point2Bearing) = SpecialOrthogonal(2)
 # Pose2Point2Bearing(x1::B) where {B <: IIF.SamplableBelief} = Pose2Point2Bearing{B}(x1)
-# FIXME, there might be something wrong with the sign here
-function getSample(cfo::CalcFactor{<:Pose2Point2Bearing})
-  return rand(cfo.factor.Z)
-end
 
-function (cfo::CalcFactor{<:Pose2Point2Bearing})(Xc, p, l)
+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(Xc[1] - 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
 

src/factors/Pose3Pose3.jl

@@ -47,11 +47,6 @@ function convert(::Type{PackedPose3Pose3}, obj::Pose3Pose3)
   return PackedPose3Pose3( convert(PackedSamplableBelief, obj.Z) )
 end
 
-
-
-
-#
-
 ##
 Base.@kwdef struct Pose3Pose3RotOffset{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
   Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)]))
@@ -73,3 +68,21 @@ function (cf::CalcFactor{<:Pose3Pose3RotOffset})(aX, p, q, bRa)
     q̂ = Manifolds.compose(M, p, b_m)
     return vee(M, q, log(M, q, q̂)) # coordinates
 end
+
+## Pose3Pose3UnitTrans Factor 
+"""
+  $(TYPEDEF)
+Pose3Pose3 factor where the translation scale is not known, ie. Pose3Pose3 with unit (normalized) translation.
+"""
+Base.@kwdef struct Pose3Pose3UnitTrans{T <: IIF.SamplableBelief} <: IIF.AbstractManifoldMinimize
+  Z::T = MvNormal(zeros(6),LinearAlgebra.diagm([0.01*ones(3);0.0001*ones(3)]))
+end
+
+getManifold(::InstanceType{Pose3Pose3UnitTrans}) = getManifold(Pose3) # Manifolds.SpecialEuclidean(3)
+
+function (cf::CalcFactor{<:Pose3Pose3UnitTrans})(X, p::ArrayPartition{T}, q) where T
+    M = getManifold(Pose3)
+    q̂ = Manifolds.compose(M, p, exp(M, getPointIdentity(M), X))
+    Xc::SVector{6,T} = get_coordinates(M, q, log(M, q, q̂), DefaultOrthogonalBasis())
+    return SVector{6,T}(normalize(Xc[1:3])..., Xc[4:6]...)
+end

test/testBearing2D.jl

@@ -34,7 +34,8 @@ rs = [0, -pi/4, -pi/2, -3pi/4, -pi, 3pi/4, pi/2, pi/4, pi/4, -pi/4]
 push!(rs, pi/4 - atan(3,4))
 
 q = [5., 5]
-m = [pi/4]
+SO2 = SpecialOrthogonal(2)
+m = hat(SO2, getPointIdentity(SO2), [pi/4])
 
 f = Pose2Point2Bearing(Normal(pi/4,0.05))
 
@@ -59,7 +60,9 @@ res = calcFactorResidualTemporary(f, (Pose2, Point2), [], (xi, xj))
 f = Pose2Point2Bearing(Normal(pi,0.001))
 xi = ArrayPartition([0.,0], [1. 0; 0 1])
 xj = [-1, -0.001]
-res = calcFactorResidualTemporary(f, (Pose2, Point2), [pi], (xi, xj))
+
+m = hat(SO2, getPointIdentity(SO2), [pi])
+res = calcFactorResidualTemporary(f, (Pose2, Point2), m, (xi, xj))
 @test isapprox(res, [-0.001], atol=1e-3)
 
 ##
@@ -200,9 +203,9 @@ lmp_noise = Matrix(Diagonal([0.01;0.01].^2))
 fg = initfg()
 
 # landmarks
-addVariable!(fg, :l1, Point2)
-addVariable!(fg, :l2, Point2)
-addVariable!(fg, :l3, Point2)
+addVariable!(fg, :l1, RoME.Point2)
+addVariable!(fg, :l2, RoME.Point2)
+addVariable!(fg, :l3, RoME.Point2)
 
 addFactor!(fg, [:l1], PriorPoint2(MvNormal([-10.0;1.0-10.0],lmp_noise)), graphinit=false)
 addFactor!(fg, [:l2], PriorPoint2(MvNormal([-10.0+sqrt(3)/2;-0.5-10.0],lmp_noise)), graphinit=false)