diff --git a/Project.toml b/Project.toml index d34b0aff..82ce4191 100644 --- a/Project.toml +++ b/Project.toml @@ -50,7 +50,7 @@ Requires = "1.0" Rotations = "0.13, 1" StaticArrays = "1" TensorCast = "0.3.3, 0.4" -TransformUtils = "0.2.10" +TransformUtils = "0.2.11" julia = "1.4" [extras] diff --git a/src/Deprecated.jl b/src/Deprecated.jl index 9e1d5f56..4683586a 100644 --- a/src/Deprecated.jl +++ b/src/Deprecated.jl @@ -88,6 +88,15 @@ end ## Remove as part of Manifolds.jl consolidation, #244 ##============================================================================== +export veePose3, veePose + +function veePose3(s::SE3) + TransformUtils.veeEuler(s) +end +function veePose(s::SE3) + TransformUtils.veeEuler(s) +end + # getManifolds(fctType::InstanceType{Pose2Pose2}) = getManifolds(getDomain(fctType)) # getManifolds(fctType::InstanceType{Pose2Point2}) = getManifolds(getDomain(fctType)) diff --git a/src/OdometryUtils.jl b/src/OdometryUtils.jl index 7601f5a4..de3b41bb 100644 --- a/src/OdometryUtils.jl +++ b/src/OdometryUtils.jl @@ -132,6 +132,18 @@ function extractDeltaOdo(XX, YY, TH) end +# DX = [transx, transy, theta] +function addPose2Pose2!(retval::Array{<:Real,1}, x::Array{<:Real,1}, dx::Array{<:Real,1}) + X = SE2(x) + DX = SE2(dx) + se2vee!(retval, X*DX) + nothing +end +function addPose2Pose2(x::Array{<:Real,1}, dx::Array{<:Real,1}) + retval = zeros(3) + addPose2Pose2!(retval, x, dx) + return retval +end ## Previous methods diff --git a/src/RoME.jl b/src/RoME.jl index b22dcda5..7f9000eb 100644 --- a/src/RoME.jl +++ b/src/RoME.jl @@ -67,8 +67,6 @@ export addAutoLandmBR!, projNewLandm!, malahanobisBR, - veePose3, - veePose, \, RangeAzimuthElevation,